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

Abstract

This route generation device comprises a map creation unit (142), a coupling relationship registration unit (143), and a route creation unit (144). When an environment map composed of a three-dimensional point cloud is divided into a plurality of grids, the map creation unit (142) sets each grid among the plurality of grids as a target grid, and creates a planar map indicating at least a portion of a planar region extracted from the target grid. The coupling relationship registration unit (143): sets two plane regions that are near each other in the planar map as a target proximity plane pair; determines, on the basis of the structure of a movement mechanism of a target moving body and a geometric structure between two planar regions included in the target proximity plane pair, whether the target moving body can be stepped between two planar regions on the basis of the structure of the moving mechanism of the target moving body and a geometric structure between two planar regions included in the target proximity plane pair; and registers the coupling relationship of the target proximity plane pair when the target moving body can travel between the two 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 registered coupling relationships.

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 traversability of the autonomous mobile robot's movement mechanism, but the range to be determined is limited to the range in which the autonomous mobile robot's sensor can detect three-dimensional data. Therefore, such path generation programs do not consider whether a destination position beyond the sensor's detection range can be reached before movement, resulting in a problem of taking a detour to reach the destination position if the destination position is not located at the end of a route based on the detected three-dimensional data. The present disclosure aims to create a route for an autonomous mobile robot that does not take a detour by determining whether planar areas in an area in which the autonomous mobile robot operates are navigable by taking into account constraints such as the traversability of the autonomous mobile robot's movement mechanism, describing the connections between the planar areas in an environmental map if navigability is possible, and creating a route that can reach the destination position from the movement start position by tracing the described connections.

[0006] The path generation device of the present disclosure comprises: a map creation unit that, 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 planes included in the target area are divided, designates each grid of the plurality of grids as a target grid, and extracts a planar area from the target grid when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, and creates a planar map showing at least a portion of the extracted planar area; a connection relationship registration unit that designates each two adjacent planar areas in 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 movement mechanism of the target moving body and the geometric structure 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; and a path creation unit that creates a path for the target moving body from a movement start position on the planar map to a movement target position on the planar map based on each registered connection relationship.

[0007] According to the present disclosure, a connection relationship registration unit determines whether a target mobile object can traverse between two planar areas included in a target adjacent planar pair based on the structure of the target mobile object's movement mechanism and the geometric structure between the two planar areas included in the target adjacent planar pair in a planar map showing planar areas in the target area, and registers the connection relationship of the target adjacent planar pair if the target mobile object can traverse between the two planar areas included in the target adjacent planar pair. A path creation unit creates a path for the target mobile object from a movement start position to a movement destination position based on each registered connection relationship. Here, the target mobile object may be an autonomous mobile robot. Therefore, according to the present disclosure, for planar areas in an area in which the autonomous mobile robot operates, the unit determines whether movement between the planar areas is possible, taking into account constraints such as the traversal capability of the autonomous mobile robot's movement mechanism. If movement between the planar areas is possible, the unit describes the connection relationship between the planar areas on an environmental map. A route that can reach from the movement start position to the movement destination position is created by tracing the described connection relationship, thereby creating a route for the autonomous mobile robot that does not involve detours.

[0008] 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 for explaining the processing of the path 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. 1B is a diagram for explaining the processing of the path 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. 1C is a diagram for explaining the processing of the path 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. 1D is a diagram showing a route list 51 according to the first embodiment. FIG. 1E is a diagram showing an example of the hardware configuration of a control device 1 according to a modification of the first embodiment. FIG. 1F is a diagram showing a mechanism of a quadruped walking robot 2A according to a second embodiment. FIG. 1G is a diagram showing a range that can be set as the position of a foot 265FR according to the second embodiment. FIG. 1H is a diagram for explaining the processing of a connection relationship registration unit 143 according to the second embodiment. FIG. 1H is a diagram for explaining the processing of a connection relationship registration unit 143 according to the second embodiment. 10A and 10B are diagrams for explaining the processing of the path creation unit 144 according to embodiment 2. 10B are diagrams for explaining the processing of the path creation unit 144 according to embodiment 2, 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. 10C are diagrams for explaining the processing of the path creation unit 144 according to embodiment 2, 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.10A and 10B are diagrams for explaining the processing of the path creation unit 144 according to embodiment 2, 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. 10B is a diagram for explaining the processing of the path creation unit 144 according to embodiment 2, where (a) is a table corresponding to the end of the seventh iteration and (b) is a table corresponding to the end of the eighth iteration. FIG. 10C is a diagram for explaining the processing of the path creation unit 144 according to embodiment 2. FIG. 10D is a diagram showing path lists according to embodiment 2, where (a) is a table showing path list 52 and (b) is a table showing path list 53. FIG. 10D is a diagram for explaining the processing of the connection relationship registration unit 143 according to embodiment 3. FIG. 10E is a flowchart showing the operations of the connection relationship registration unit 143 and the path creation unit 144 according to embodiment 3.

[0009] 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.

[0010] 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.

[0011] 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.

[0012] 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.

[0013] 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.

[0014] 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.

[0015] 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.

[0016] 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."

[0017] 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.

[0018] 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 at least a portion of the extracted planar areas. As a specific example, 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.

[0019] The connection relationship registration unit 143 defines each of two adjacent planar areas on the planar map as a target adjacent planar pair, and determines whether the target moving object can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the moving mechanism of the target moving object and the geometric structure between the two planar areas included in the target adjacent planar pair. If the target moving object can traverse between the two planar areas included in the target adjacent planar pair, the connection relationship of the target adjacent planar pair is registered. A specific example of the geometric structure between the two planar areas included in the target adjacent planar pair includes the height difference between the two planar areas included in the target adjacent planar pair. In this case, the connection relationship registration unit 143 determines whether the target moving object can reach the other planar area of ​​the target adjacent planar pair from one planar area of ​​the target adjacent planar pair based on the structure of the moving mechanism of the target moving object and the height difference between the two planar areas included in the target adjacent planar pair. Then, the connection relationship registration unit 143 determines that the target moving body can traverse between the two planar areas included in the target nearby plane pair if the target moving body can reach the other planar area of ​​the target nearby plane pair from one planar area of ​​the target nearby plane pair.

[0020] 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.

[0021] 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 movement 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.

[0022] 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.

[0023] 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.

[0024] 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.

[0025] 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 moving 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.

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

[0027] The moving mechanism 26 is formed of tires, for example, and transmits the driving force received from the actuators 25 to the ground in contact with the moving mechanism 26, thereby enabling the robot device 2 to accelerate or decelerate. The moving 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 moving mechanisms 26. In this example, each pair of the two moving mechanisms 26 shares a tire axis, and the two pairs of actuators 25 transmit different driving forces to each moving mechanism 26, thereby enabling movement such as translation and rotation.

[0028] 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.

[0029] 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.

[0030] 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.

[0031] First, the surrounding environment detection unit 273 determines, as an obstacle, an object in a location of the 3D local point cloud data that does not match the 3D global point cloud data used in matching by the self-position estimation unit 272. 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 temporarily regard the point cloud data classified as pedestrians, bicycles, or the like as data corresponding to an obstacle.

[0032] 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.

[0033] 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.

[0034] 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.

[0035] 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.

[0036] 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.

[0037] 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.

[0038] 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.

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

[0040] 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).

[0041] 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 .

[0042] 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.

[0043] 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.

[0044] 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.

[0045] 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.

[0046] ***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.

[0047] 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.

[0048] (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.

[0049] 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.

[0050] (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.

[0051] (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.

[0052] (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.

[0053] 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 mobile 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.

[0054] 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.

[0055] (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.

[0056] (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.

[0057] 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.

[0058] 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.

[0059] (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.

[0060] (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.

[0061] 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.

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

[0063] 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.

[0064] (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.

[0065] 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."

[0066] (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.

[0067] (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.

[0068] 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.

[0069] 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.

[0070] 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.

[0071] (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.

[0072] (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.

[0073] 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.

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

[0075] 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.

[0076] (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.

[0077] (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.

[0078] (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.

[0079] (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.

[0080] (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.

[0081] (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.

[0082] (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.

[0083] (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.

[0084] (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.

[0085] (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".

[0086] (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.

[0087] (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.

[0088] 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.

[0089] (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.

[0090] (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.

[0091] 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).

[0092] 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).

[0093] 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.

[0094] (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.

[0095] 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.

[0096] 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.

[0097] ***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.

[0098] ***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.

[0099] 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.

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

[0101] 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.

[0102] Second Embodiment The following mainly describes the differences from the above-described embodiment with reference to the drawings.

[0103] *** Description of Configuration *** In the first embodiment, it is assumed that the robot device 2 is a wheeled robot, and a connection relationship is registered when the difference in height between adjacent candidate movement planes is equal to or less than the traversable threshold. Here, when the robot device 2 has a movement mechanism with legs, such as a bipedal or quadrupedal movement mechanism, not only is the possibility of traversing adjacent candidate movement planes improved, but even if the two candidate movement planes are not adjacent, the robot device 2 may be able to traverse from one to the other of the two candidate movement planes. The path generation system 90 according to the second embodiment is similar to the path generation system 90 according to the first embodiment, except for the configuration of the robot device 2.

[0104] When one or more planar areas classified as obstacle planes exist between one planar area of ​​the target nearby plane pair and the other planar area of ​​the target nearby plane pair, the connection relationship registration unit 143 according to this embodiment determines whether the target moving object can overcome the obstacle plane group and move from one planar area of ​​the target nearby plane pair to the other planar area of ​​the target nearby plane pair based on the height of each planar area included in the obstacle plane group, the height of the obstacle in each planar area included in the obstacle plane group, and the structure of the movement mechanism of the target moving object. Then, when the connection relationship registration unit 143 determines that the target moving object can overcome the obstacle plane group and move from one planar area of ​​the target nearby plane pair to the other planar area of ​​the target nearby plane pair, it registers the connection relationship of the target nearby plane pair. The target moving object may be a robot device having multiple legs as its movement mechanism.

[0105] The path creation unit 144 according to the present embodiment creates a path by preferentially selecting, from among the registered connection relationships, connection relationships with a relatively low corresponding movement difficulty. The path creation unit 144 may set a speed of a target moving object corresponding to a connection relationship with a relatively high corresponding movement difficulty to be lower than the speed of a target moving object corresponding to a connection relationship with a relatively low corresponding movement difficulty. Note that, between two planar regions corresponding to each registered connection relationship, a movement difficulty is set based on the geometric structure between the two planar regions corresponding to each registered connection relationship.

[0106] 21 is a diagram showing an example of the mechanism of a quadruped walking robot 2A in which each leg has three degrees of freedom. CoG is an abbreviation for Center of Gravity. In the quadruped walking robot 2A, as shown in the figure, the front right leg (FR), front left leg (FL), rear right leg (RR), and rear left leg (RL) are connected to the trunk link 261 via first motors 251FR, 251FL, 251RR, and 251RL, respectively, so that their axes are rotatable parallel to the X-axis of the trunk link 261. The first link 262FR, the first link 262FL, the first link 262RR, and the first link 262RL are fixedly connected to the first motors 251 so that the longitudinal direction of each first link 262 is directed in the radial direction of the rotation shaft of the first motor 251FR, the first motor 251FL, the first motor 251RR, and the first motor 251RL. The rotation shafts of the second motors 252FR, the second motor 252FL, the second motor 252RR, and the second motor 252RL are connected to each first link 262 so as to be rotatable and parallel to the longitudinal direction of the first link 262FR, the first link 262FL, the first link 262RR, and the first link 262RL, respectively. The second link 263FR, the second link 263FL, the second link 263RR, and the second link 263RL are connected to the second motors 252 so that the longitudinal direction of each second link 263 faces the radial direction of the rotation axis of the second motor 252FR, the second motor 252FL, the second motor 252RR, and the second motor 252RL, and are rotatable. The third motor 253FR, the third motor 253FL, the third motor 253RR, and the third motor 253RL are connected to the second link 263FR, the second link 263FL, the second link 263RR, and the second link 263RL, respectively. The rotation shafts of the third motors 253 are connected in parallel to the rotation shafts of the second motors 252FR, 252FL, 252RR, and 252RL, respectively.Third link 264FR, third link 264FL, third link 264RR, and third link 264RL are connected to each third motor 253 so that the longitudinal direction of each third link 264 is directed in the radial direction of the rotation axis of third motor 253FR, third motor 253FL, third motor 253RR, and third motor 253RL. Foot 265FR, foot 265FL, foot 265RR, and foot 265RL are fixedly connected to each third link 264 at one end of the longitudinal direction of third link 264FR, third link 264FL, third link 264RR, and third link 264RL, respectively. First motor 251 and second motor 252 each correspond to the hip joint of the animal. Third motor 253 corresponds to the knee joint of the animal. Each of the first motors 251, second motors 252, and third motors 253 is equipped with a rotation sensor, a torque sensor, and the like (not shown). The robot device 2 calculates and controls the state of the legs, such as their posture, based on information acquired by these sensors. The lengths of each of the first links 262, second links 263, and third links 264 (including the feet 265) for each leg are denoted as L1, L2, and L3, respectively.

[0107] ***Explanation of Operation*** In order to consider the walking ability of the quadruped walking robot 2A, the forward kinematics of static walking in which the quadruped walking robot 2A walks with one leg lifted from the ground as a swing leg will be calculated. As a specific example, consider a case in which the quadruped walking robot 2A takes one step by static walking with the foot 265FR (front right leg) as a swing leg. If the connection point between the first motor 251FR and the first link 262FR on the front right leg is taken as the origin, the joint angles of the first motor 251FR, the second motor 252FR, and the third motor 253FR are each θ １ and θ ２ and θ ３ When this is the case, the three-dimensional coordinates (X ＦＲ , Y ＦＲ , Z ＦＲ ) is as shown in [Equation 1]. ｋ = cosθ ｋ , S ｋ = sin θ ｋ (k=1, 2, 3).

[0108]

[0109] FIG. 22 is a diagram showing a range that can be set as the position of the foot 265FR of the front right leg. １ and θ ２ and θ ３ If the foot 265FR is not limited, the position of the foot 265FR can be set within the range of an ellipsoid 28 centered on the origin. However, in reality, there are limitations on the range of motion of the joint angles, just like the hip and knee joints of an animal. Furthermore, if the posture of the foot 265FR when it touches the ground is inappropriate, and the other legs are in a free position, this can cause the quadruped robot 2A to lose balance or slip. The range of the optimal position and posture of the foot 265FR when it touches the ground is calculated taking into account various parameters such as the inclination of the road surface. However, the inclination of the road surface can be limited by conditions such as the candidate movement plane. Therefore, to simplify the calculation, the range that can be set as the position of the foot 265FR is limited by a cube (foot setting cube 29) as shown in FIG. 22. In the foot setting cube 29, a range that is symmetrical with respect to the YZ plane is set in the direction of travel (X direction in the figure) so that the quadruped robot 2A can move equally forward and backward. In the left-right direction (Y direction in the figure), because the range of motion of the first motor 251FR is narrow, a narrow range is set centered on the plane of movement of the foot 265FR when the shaft of the first motor 251FR is horizontal. In the height direction (Z direction in the figure), a range is set such that the range above the XY plane is smaller than the range below the XY plane, taking into consideration that the position of the third motor 253FR can be set higher than the foot 265FR so that the shaft of the third motor 253FR does not interfere with obstacles along the way. As described above, the quadruped walking robot 2A has a possibility of traversing a candidate movement plane at least partially within the range of the foot setting cube 29 when at least a portion of the candidate movement plane that is not adjacent to the candidate movement plane on which the foot 265FR is currently in contact with ground is within the range of the foot setting cube 29.

[0110] For each grid used to create the candidate movement planes and obstacle planes, assuming a wheeled robot, it is desirable that the length of one side of each grid be equal to or greater than the greater of the total length or width of the robot device 2 so that the robot device 2 can fit within each grid. However, for the quadrupedal robot 2A, it is sufficient for the foot 265 to be able to stably land on the ground. Therefore, it is desirable that the length of one side of each grid be approximately the greater of the length or width of the bottom surface of the foot 265, plus a margin of several times. Here, since the grid sizes differ between the wheeled robot and the quadrupedal robot 2A, a plane list may be created separately from the plane list 41 for the wheeled robot. However, newly created candidate movement planes and obstacle planes may be registered in the plane list 41 by dividing only the obstacle planes for the wheeled robot into integer equal parts so that the length of one side after division is greater than the length of one side of the grid for the quadrupedal robot 2A. Furthermore, different connection relationships may be registered between the plane list 41 for the wheeled robot and the plane list 41 for the quadrupedal robot 2A.

[0111] 23 and 24 show an example of a modified extended plane list 43 created by modifying the extended plane list 42 for the quadruped walking robot 2A. The modified extended plane list 43 is shown divided into two diagrams. The modified extended plane list 43 corresponds to a planar map. FIG. 25 shows a schematic diagram corresponding to the modified extended plane list 43 shown in FIGS. 23 and 24. FIG. 25 shows connection relationships common to wheeled robots and quadruped walking robots, as well as connection relationships that apply only to quadruped walking robots. In this example, the obstacle plane corresponding to ID=6 is divided into four planes corresponding to IDs=17 to 20. Of the divided planes, the planes corresponding to IDs=17 and 20 are determined to be candidate movement planes. The connection relationships of the quadruped walking robot 2A share some commonalities with the connection relationships of wheeled robots. Furthermore, compared to the traversal threshold for the wheeled robot, the traversal threshold has increased from 0.25 m to 0.5 m (the maximum value in the Z-axis direction of the foot setting cube 29). Therefore, ID=10 and ID=11 are newly connected. Furthermore, ID=17 and ID=20 each correspond to a candidate movement plane, and ID=5 and ID=17, ID=2 and ID=17, and ID=20 and ID=7 are adjacent to each other. Furthermore, for each of these pairs of IDs, the height difference between the corresponding planes is equal to or less than the traversal threshold, so these pairs of IDs are connected. For ID=2 and ID=20, and ID=17 and ID=7, it is determined that the right leg (both front and rear) can climb over them, and these pairs of IDs are connected. For ID=5 and ID=20, it is determined that the left leg (both front and rear) can climb over them, and this pair of IDs is connected. In addition, if a target plane, which is a movable plane that is not adjacent to a certain movable plane, exists inside the foot setting cube 29, and the maximum obstacle height of the obstacle plane between the certain movable plane and the target plane does not exceed the maximum value in the Z-axis direction of the foot setting cube 29, it is determined that the quadrupedal robot 2A can overcome the obstacle plane with respect to the certain movable plane and the target plane.

[0112] When registering a connection relationship for the quadrupedal robot 2A, a connection relationship may be registered when only one of the right leg and the left leg can be climbed over. As a specific example, when the quadrupedal robot 2A has its front right leg placed on a movement candidate plane corresponding to a certain ID determined to have a connection relationship for the right leg, the only leg that can be used as a free leg when moving to the next movement candidate plane that is in front of the quadrupedal robot 2A is the front left leg. Therefore, when there is a movement candidate plane on which the quadrupedal robot 2A can move alternately using the left and right feet, or on which the quadrupedal robot 2A can move using either the left or right leg, depending on the state of the legs of the quadrupedal robot 2A, a connection relationship between the movement candidate plane corresponding to the certain ID and the next movement candidate plane may be registered.

[0113] FIG. 26 is a schematic diagram showing the search order of the grid map shown in FIG. 25. FIGS. 27 to 31 show the transitions of the unsearched list and searched list from the start to the end of the search process taking travel difficulty into account. In the first embodiment, cost calculations were performed taking into account only Manhattan distance. However, smoother travel may be expected by selecting a route that does not require avoiding obstacle planes or a route with smaller steps, which is in good condition and easy to travel. Therefore, in this embodiment, [Formula 2] is adopted as the formula for calculating the cost of a target ID.

[0114] [Formula 2] C (TID) = C (PID) + D (TID, PID) + M (TID, GID) - M (PID, GID)

[0115] In [Formula 2], the cost of "+1" that was increased when moving to the next movement candidate plane in [Formula 1] is replaced with the movement difficulty D(TID, PID) between the target ID and parent ID in a connection relationship. D(TID, PID) is defined by [Formula 2]

[0116]

[0117] Furthermore, IDs 17 to 20 were generated by dividing ID 6. Therefore, M(17, GID) = M(18, GID) = M(19, GID) = M(20, GID). If the path generation unit 144 employs [Formula 2] that takes into account the travel difficulty, as can be seen from the searched list at the end of the ninth iteration shown in FIG. 31 , it is possible to obtain coordinate information for a route connecting the travel start position to the travel destination position, i.e., IDs 1 → 2 → 3 → 7 → 8 → 11, which does not cross any obstacle planes and traverses relatively low steps. On the other hand, if the path generation unit 144 employs [Formula 1] that does not consider the travel difficulty, the cost of moving to ID 7 at the end of the fourth iteration of step S640 would be 1, not 1.5. Therefore, a route that crosses the obstacle planes is selected. Furthermore, at the end of the eighth iteration of step S640 (although it is the eighth in the diagram, it would be the sixth if the movement difficulty level is not taken into account from the initial state), the cost of moving to ID=11 is 1, not 2. Therefore, a route that traverses a 0.4 m step is selected, and as a result, coordinate information for the route connecting the movement start position to the movement target position, ID=1→5→17→7→10→11, is acquired.

[0118] 32(a) shows a specific example of a route list 52 in which speed information is assigned to the coordinate information of a route when the movement difficulty level shown in FIG. 31 is taken into consideration. FIG. 32(b) shows a specific example of a route list 53 in which speed information is assigned to the coordinate information of a route when the movement difficulty level is not taken into consideration. For a route in which the movement difficulty level is taken into consideration, when the quadrupedal walking robot 2A does not climb over an obstacle plane and when the quadrupedal walking robot 2A traverses a relatively low step, it is desirable to assign speed information indicating that the quadrupedal walking robot 2A gradually accelerates from the movement start position, decelerates before a corner, accelerates again after passing the corner to return to the rated speed, and gradually decelerates so that the speed becomes zero before the movement target position, as in the first embodiment. (a) of Figure 32 shows the speed setting when the absolute upper limit of the speed change for moving between one grid is 2 km / h, the upper limit of the speed at a corner is 3 km / h, and the maximum speed is 3.5 km / h. On the other hand, for a route that does not take into account the travel difficulty, or for a route that takes into account the travel difficulty and requires the robot to overcome an obstacle plane or traverse a relatively high step, in addition to the absolute upper limit of the speed change, the upper limit of the speed at a corner, and the constraint on the maximum speed, the maximum speed when traversing the obstacle plane and the maximum speed when traversing a high step can be set to reduce the risk of control on a route with high travel difficulty, such as colliding with an obstacle on the obstacle plane or failing to traverse a high step. In (b) of Figure 32, in addition to the speed setting similar to that of embodiment 1, the maximum speed when traversing an obstacle plane is set to 2 km / h, and the maximum speed when traversing a high step is set to 1 km / h. Therefore, the initial speed at ID = 1 is 2 km / h, which is the absolute upper limit of the speed change. At ID=5, the quadruped walking robot 2A accelerates to 3 km / h, which is the upper speed limit for turning a corner. At ID=17, the quadruped walking robot 2A decelerates to 2 km / h, which is the maximum speed for turning a flat obstacle, in order to turn over the flat obstacle. At ID=7, the quadruped walking robot 2A decelerates to 3 km / h, which is the upper speed limit for turning a corner.At ID=10, the quadruped walking robot 2A decelerates to 1 km / h because this is a candidate movement plane immediately before climbing over a high step. The speed setting at ID=11, which is the movement target position, is 0 km / h. In this example, the maximum speed when climbing over an obstacle plane and the maximum speed when climbing over a relatively high step are each set to a constant value. However, these maximum speeds may be set to variable maximum speeds, such as a maximum speed proportional to or inversely proportional to the maximum obstacle height in the obstacle plane to be climbed or the height of the step.

[0119] ***Explanation of Effects of Embodiment 2*** As described above, according to this embodiment, it is possible to register connection relationships for a quadruped walking robot. Furthermore, according to this embodiment, it is possible to create a route taking into account the degree of difficulty of movement.

[0120] Third Embodiment Hereinafter, differences from the above-described embodiments will be mainly described with reference to the drawings.

[0121] ***Configuration Description*** In the first embodiment, a method for extracting planar areas that the robot device 2 can enter using a histogram is described. However, in some cases, a candidate movement plane on which the robot device 2 can be placed may have all of its surrounding planar areas as obstacle planes, or may be surrounded by a grid with no planar areas, making it impossible to register a connection relationship with an external candidate movement plane. Therefore, it is desirable to remove such candidate movement planes from the plane list 41 so that they cannot be set as a movement start position or a movement target position.

[0122] The route generation system 90 according to the third embodiment is similar to the route generation system 90 according to the first embodiment.

[0123] The connection relationship registration unit 143 according to this embodiment determines whether a route for the target moving object from the first point to the second point can be created. If the connection relationship registration unit 143 determines that a route for the target moving object from the first point to the second point cannot be created, the connection relationship registration unit 143 classifies all planar areas that can be reached from the first point by tracing the registered connection relationships as non-navigable planes. Each of the first point and the second point is a point on a planar area that is not classified as an obstacle plane shown on the planar map.

[0124] When the route creation unit 144 determines that it is not possible to create a route for the target moving body from the first point to the second point, it creates a route from the first point to a point on a planar area that is not classified as an obstacle plane shown on the planar map and that is relatively close to the second point.

[0125] ***Explanation of Operation*** Figure 33 is a schematic diagram showing an example of information in the plane list 41 in the form of a grid map, including a travel candidate plane for which a connection relationship cannot be registered. Figure 34 shows the extended plane list 42 corresponding to the grid map shown in Figure 33. In this example, the travel candidate plane corresponding to ID = 12 is an untravelable plane because no connection relationship with other travel candidate planes is registered. On the other hand, travel candidate planes such as ID = 13, 14, and 15, which have connection relationships registered but cannot be connected to routes from travel candidate planes other than the travel candidate planes corresponding to these IDs, are also untravelable planes. However, the removal of multiple travel candidate planes isolated from other multiple travel candidate planes cannot be simply determined from the extended plane list 42.

[0126] 35 is a flowchart showing the flow of processing for determining, as an immovable plane, a travel candidate plane to which a route cannot be connected from the extended plane list 42. This processing will be described with reference to FIG.

[0127] (Step S710) The processes from step S710 to step S820 are repeated processes that are executed the same number of times as the number of rows in the extended plane list 42. In the repeated processes, the connection relationship registration unit 143 selects rows indicated in the extended plane list 42 one by one, sets the plane corresponding to the selected row as a target plane, and classifies the target plane as either a movable plane or an immovable plane.

[0128] (Step S720) The processing in this step is processing for determining whether the target plane is a movement candidate plane or an obstacle plane. If the target plane is an obstacle plane, the connection relationship registration unit 143 proceeds to processing the next row in the extended plane list 42 without performing any processing.

[0129] (Step S730) The processing of this step is processing for determining whether the target plane is a movement candidate plane that has been determined as either a movable plane or an immovable plane. If the target plane is a movement candidate plane that has been determined as either a movable plane or an immovable plane, the connection relationship registration unit 143 does not perform any processing and proceeds to processing of the next row in the extended plane list 42. Otherwise, the connection relationship registration unit 143 proceeds to step S740.

[0130] (Step S740) The connection relationship registration unit 143 checks the number of registered connection relationships related to the target plane. If the number of registered relationships is 1 or more, the connection relationship registration unit 143 proceeds to step S760. If the number of registered relationships is 0, the connection relationship registration unit 143 proceeds to step S750.

[0131] (Step S750) The connection relationship registration unit 143 determines that the target plane is an immovable plane, and proceeds to processing of the next row in the extended plane list 42.

[0132] (Step S760) The path creation unit 144 executes the path search process shown in FIGS. 13 and 14 by setting a target plane for the movement target position and setting a movable plane that is closest to the target plane for the movement start position. One of the movement start position and the movement target position corresponds to a first location, and the other corresponds to a second location. When there are no movable planes, it is necessary to set one of the candidate planes as the movable plane in advance in order to execute the process of this step. Here, it is appropriate to set the candidate plane for movement that corresponds to the current position of the robot device 2 as the movable plane. Furthermore, when selecting the closest movable plane, the path creation unit 144 may select the movable plane that is closest in terms of Manhattan distance ignoring the connection relationships (if there are multiple closest movable planes, select any one of the movable planes).

[0133] (Step S770) Regardless of whether the route search process in step S760 ends with "arrival completed" or "non-arrival completed," all of the travel candidate planes indicated in the searched list and unsearched list calculated during the process are planes reachable from the travel start position. Therefore, the connection relationship registration unit 143 registers all of the travel candidate planes indicated in the searched list and unsearched list as travel possible planes.

[0134] (Step S780) If the result of the route search process in step S760 is "arrival end", the connection relationship registration unit 143 proceeds to the process of the next row, and if the result is "non-arrival end", the connection relationship registration unit 143 proceeds to step S790.

[0135] (Step S790) The connection relationship registration unit 143 determines that the target plane is an unmovable plane because it has been confirmed that it is not possible to connect a path from the movement start position, which is a movable plane, to the target plane.

[0136] 13 and 14 by setting a target plane (non-movable plane) for the movement start position, and setting a movable plane that is closest to the target plane for the movement target position. The closest movable plane may be the same as or different from the closest movable plane in step S760.

[0137] (Step S810) Since all of the candidate planes indicated by the searched list and the unsearched list calculated until the path search process in step S800 ends with "non-reachability", are unreachable from the target position, which is a movable plane, the connection relationship registration unit 143 registers all of the candidate planes indicated by the searched list and the unsearched list as non-movable planes. This makes it possible to collectively discover non-movable planes associated with one non-movable plane.

[0138] (Step S820) This step is the end point of each iteration in the repeating process started in step S710. The connection relationship registration unit 143 ends the processing of the flowchart when processing has been completed for all rows in the extended plane list 42, and otherwise transitions to step S710.

[0139] The process of the flowchart described above is a process for generating an environmental map simultaneously with self-localization, as in SLAM. When a new travel candidate plane is generated and the augmented plane list 42 is updated, the process of the flowchart is performed to remove travel candidate planes that cannot be set as travel target positions from the augmented plane list 42. However, when the range of the environmental map is expanded, a certain unmovable plane may become a movable plane due to the discovery of a detour. Therefore, when the process shown in FIG. 35 is executed again, it is recommended to re-determine all areas previously determined to be unmovable planes as undetermined travel candidate planes. Note that movable planes do not need to be re-determined. Furthermore, when the robot device 2 is caused to automatically generate an environmental map, a route search process similar to steps S760 and S800 is performed to generate a route from the current position of the robot device 2 to a movable plane closest to the unmovable plane, thereby increasing the likelihood of finding a detour that allows the unmovable plane to be re-determined as a movable plane.

[0140] ***Explanation of Effects of Third Embodiment*** As described above, according to this embodiment, each plane is classified as a movable plane or an immovable plane, so that a route can be created more efficiently.

[0141] ***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 3, and various modifications are possible as needed. The procedures described using flowcharts, etc., can be modified as appropriate.

[0142] 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 Moving 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, 2A Quadruped walking robot, 251, 251FR, 251FL, 251RR, 251RL First motor, 252, 252FR, 252FL, 252RR, 252RL Second motor, 253, 253FR, 253FL, 253RR, 253RL Third motor, 261 Body link, 262, 262FR, 262FL, 262RR, 262RL First link, 263, 263FR, 263FL, 263RR, 263RL Second link, 264, 264FR, 264FL, 264RR, 264RL Third link, 265, 265FR, 265FL, 265RR, 265RL Foot, 29 Foot setting cube, 31 3D global point cloud data, 32, 33, 34 Lattice, 41 Plane list, 42 Extended plane list, 43 Modified extended plane list, 51, 52, 53 Path list, 90 Path generation system, 101 Processor, 102 memory, 103 auxiliary storage device, 104 display interface, 105 communication interface, 106 input interface, 107 display, 108 wired LAN port, 109 keyboard, 110 mouse, 111 processing circuit.

Claims

1. A path generation device comprising: a map creation unit that, 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 planes included in the target area are divided, designates each grid of the plurality of grids as a target grid, and extracts a planar area from the target grid when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, and creates a planar map showing at least a portion of the extracted planar area; a connection relationship registration unit that designates each two adjacent planar areas in the planar map as a target adjacent planar pair, and determines whether the target moving object can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the movement mechanism of the target moving object and the geometric structure between the two planar areas included in the target adjacent planar pair, and registers the connection relationship of the target adjacent planar pair if the target moving object can traverse between the two planar areas included in the target adjacent planar pair; and a path creation unit that creates a path for the target moving object from a movement start position on the planar map to a movement target position on the planar map based on each registered connection relationship.

2. The path generation device of claim 1, wherein the map creation unit includes information indicating the height of each planar area shown in the planar map, the geometric structure between the two planar areas included in the target nearby planar pair includes a height difference between the two planar areas included in the target nearby planar pair, and the connection relationship registration unit determines whether the target moving body can reach the other planar area of the target nearby planar pair from one planar area of the target nearby planar pair based on the structure of the movement mechanism of the target moving body and the height difference between the two planar areas included in the target nearby planar pair, and determines that the target moving body can traverse between the two planar areas included in the target nearby planar pair if the target moving body can reach the other planar area of the target nearby planar pair from one planar area of the target nearby planar pair.

3. The map creation unit uses a planar area extracted from each grid from which a planar area is extracted as a target plane, and classifies the target plane as an obstacle plane if a three-dimensional point cloud exists in the grid including the target plane between the height of the target plane and a height obtained by adding a movable height threshold that is equal to or greater than the height of the target moving body to the height of the target plane, in the height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; each planar area of the target adjacent plane pair is a planar area that is not classified as an obstacle plane; the movement start position is a position on the planar area that is not classified as an obstacle plane and is indicated by the planar map; and the movement target position is a position on the planar area that is not classified as an obstacle plane and is indicated by the planar map.

4. 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 an obstacle in each planar area classified as an obstacle plane in the planar map. The path generation device of claim 3, wherein the connection relationship registration unit, when there is one or more planar areas classified as obstacle planes as an obstacle plane group between one planar area of the target nearby plane pair and the other planar area of the target nearby plane pair, determines whether the target moving body can overcome the obstacle plane group and move from one planar area of the target nearby plane pair to the other planar area of the target nearby plane pair based on the height of each planar area included in the obstacle plane group, the height of the obstacles in each planar area included in the obstacle plane group, and the structure of the movement mechanism of the target moving body, and registers the connection relationship between the two planar areas included in the target nearby plane pair if it determines that the target moving body can overcome the obstacle plane group and move from one planar area of the target nearby plane pair to the other planar area of the target nearby plane pair.

5. A path generation device as described in claim 3 or 4, wherein the connection relationship registration unit determines whether it is possible to create a path for the target moving body from a first point on a planar area not classified as an obstacle plane shown on the planar map to a second point on a planar area not classified as an obstacle plane shown on the planar map, and if it determines that it is not possible to create a path for the target moving body from the first point to the second point, classifies all planar areas that can be reached from the first point by tracing each registered connection relationship as non-travelable planes.

6. A path generation device as described in any one of claims 3 to 5, wherein the connection relationship registration unit determines whether or not it is possible to create a path for the target moving body from a first point on a planar area not classified as an obstacle plane shown on the planar map to a second point on a planar area not classified as an obstacle plane shown on the planar map, and when it is determined that it is not possible to create a path for the target moving body from the first point to the second point, the path creation unit creates a path from the first point to a point on a planar area not classified as an obstacle plane shown on the planar map that is relatively close in distance to the second point.

7. A path generation device according to any one of claims 1 to 6, wherein a degree of difficulty of movement is set between two planar areas corresponding to each registered connection relationship based on the geometric structure between the two planar areas corresponding to each registered connection relationship, and the path creation unit creates the path by preferentially selecting connection relationships with a relatively low corresponding degree of difficulty of movement from each registered connection relationship.

8. A path generation device as described in any one of claims 1 to 7, wherein a movement difficulty level is set between two planar areas corresponding to each registered connection relationship based on the geometric structure between the two planar areas corresponding to each registered connection relationship, and the path creation unit sets the speed of the target moving body corresponding to a connection relationship with a relatively high corresponding movement difficulty level to be lower than the speed of the target moving body corresponding to a connection relationship with a relatively low corresponding movement difficulty level.

9. A path generation device according to any one of claims 1 to 8, wherein the target moving body is a robot device having a plurality of legs as a movement mechanism of the target moving body.

10. A path generation method in which, 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 planes included in the target area are divided, a computer sets each grid of the plurality of grids as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, extracts a planar area from the target grid and creates a planar map showing at least a portion of the extracted planar area; the computer sets each two adjacent planar areas on the planar map as a target adjacent planar pair, and determines whether the target moving object can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the moving mechanism of the target moving object and the geometric structure between the two planar areas included in the target adjacent planar pair, and if the target moving object can traverse between the two planar areas included in the target adjacent planar pair, registers the connection relationship of the target adjacent planar pair; and the computer creates a path for the target moving object from a movement start position on the planar map to a movement target position on the planar map based on each registered connection relationship.

11. 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 planes included in the target area are divided, a map generation process that sets each grid of the plurality of grids 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, extracts a planar area from the target grid and creates a planar map showing at least a part of the extracted planar area; a connection relationship registration process that sets each two adjacent planar areas in the planar map as a target adjacent planar pair, and determines whether the target moving object can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the moving mechanism of the target moving object and the geometric structure between the two planar areas included in the target adjacent planar pair, and registers the connection relationship of the target adjacent planar pair if the target moving object can traverse between the two planar areas included in the target adjacent planar pair; and a path generation process that creates a path for the target moving object from a movement start position on the planar map to a movement target position on the planar map based on each registered connection relationship.

Images