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

Abstract

The path generation device includes a map creation unit (142), a connection relationship registration unit (143), and a path creation unit (144). When an environmental map consisting of a 3D point cloud is divided into multiple grids, the map creation unit (142) defines each of the multiple grids as a target grid and creates a planar map showing at least a portion of a planar area extracted from the target grid. The connection relationship registration unit (143) defines each of 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 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, the connection relationship of the target adjacent planar pair is registered. The path creation unit (144) creates a path for the target moving object from a movement start position to a movement destination position based on each registered connection relationship.

Description

[Technical Field]

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

[0002] In recent years, autonomous mobile robots are expected to perform tasks such as transporting passengers and goods, or inspecting and guarding facilities, not only in limited environments such as factories and warehouses, but also in environments frequented by the general public, such as commercial facilities and urban areas. Autonomous mobile robots that perform such tasks are equipped with an appropriate mobility mechanism depending on the task. For example, for robots engaged in transportation tasks, wheeled robots are preferred because it is important to transport passengers and goods stably. On the other hand, for robots engaged in inspection and security tasks, the ability to negotiate steps and other obstacles that humans can enter is often expected, so walking robots are used in addition to wheeled robots. In this way, even if the start position and the target position of movement are the same between autonomous mobile robots with different movement mechanisms, the intermediate path for efficiently moving from the start position to the target position of movement may be different.

[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 that detects planes and obstacles from three-dimensional data to create an environmental map, and then creates a movement path using the created environmental map, as disclosed in Patent Document 1. 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 floors 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. [Prior art documents] [Patent documents]

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

[0005] Conventional path generation programs determine whether a plane is navigable, 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 3D data. Therefore, such path generation programs do not consider whether a movement target position beyond the sensor's detection range can be reached before movement, which poses a problem of the program taking a detour to reach the movement target position if the movement target position does not exist beyond the path based on the detected 3D data. The present disclosure aims to create a route for an autonomous mobile robot that does not involve detours by determining whether or not it is possible to move between planar areas that exist in an area in which the autonomous mobile robot operates, taking into account constraints such as the traversal capability of the autonomous mobile robot's movement mechanism, and if it is possible to move between planar areas, describing the connections between those planar areas on an environmental map, and creating a route that can be reached from the start position of movement to the target position of movement by following the described connections. [Means for solving the problem]

[0006] The route generation device according to the present disclosure comprises: a map creation unit that, when an environmental map consisting of a 3D point cloud measured in a target area is divided into a plurality of grids such that a plane included in the target area is divided, sets each grid of the plurality of grids as a target grid, and, when it is determined that a 3D 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 unit that defines each of two adjacent planar areas in the planar map as a target adjacent planar pair, 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; a route creation unit that creates a route of 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; Equipped with. [Effects of the Invention]

[0007] According to the present disclosure, a connection relationship registration unit determines whether a target moving object can traverse between two planar areas included in a 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 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 moving 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 moving object from the movement start position to the movement target position based on each registered connection relationship. Here, the target moving object may be an autonomous mobile robot. Therefore, according to the present disclosure, for planar areas existing in an area in which an autonomous mobile robot operates, it is determined whether or not movement between planar areas is possible, taking into account constraints such as the traversal capability of the autonomous mobile robot's movement mechanism, and if movement between planar areas is possible, the connection relationships between those planar areas are described on an environmental map, and a route that can be reached from the start position of movement to the target position of movement is created by tracing the described connection relationships, thereby making it possible to create a route for the autonomous mobile robot that does not involve detours. [Brief explanation of the drawings]

[0008] [Figure 1]FIG. 1 is a diagram showing an example of the configuration of a route generation system 90 according to the first embodiment. [Figure 2] 1 is a diagram showing an example of the hardware configuration of a control device 1 according to a first embodiment. [Figure 3] 6 is a flowchart showing the operation of a map creation unit 142 according to the first embodiment. [Figure 4] 6 is a flowchart showing the operation of a map creation unit 142 according to the first embodiment. [Figure 5] 3A to 3C are diagrams for explaining a grid division method according to the first embodiment. [Figure 6] 3A to 3C are diagrams for explaining a plane extraction method according to the first embodiment. [Figure 7] 3A to 3C are diagrams for explaining a virtual plane calculation method according to the first embodiment. [Figure 8] FIG. 3 is a diagram illustrating an obstacle determination process according to the first embodiment. [Figure 9] FIG. 4 is a diagram for explaining the processing of a map creation unit 142 according to the first embodiment. [Figure 10] FIG. 4 is a diagram for explaining the processing of a map creation unit 142 according to the first embodiment. [Figure 11] FIG. 4 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the first embodiment. [Figure 12] FIG. 4 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the first embodiment. [Figure 13] 6 is a flowchart showing the operation of a path creation unit 144 according to the first embodiment. [Figure 14] 6 is a flowchart showing the operation of a path creation unit 144 according to the first embodiment. [Figure 15] FIG. 4 is a diagram for explaining the processing of a path creation unit 144 according to the first embodiment. [Figure 16] 10A and 10B are diagrams illustrating 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. [Figure 17] 10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the first embodiment, where FIG. 10A is a table corresponding to the end of the third iteration, and FIG. 10B is a table corresponding to the end of the fourth iteration. [Figure 18]10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the first embodiment, where FIG. 10A is a table corresponding to the end of the fifth iteration, and FIG. 10B is a table corresponding to the end of the sixth iteration. [Figure 19] FIG. 5 is a diagram showing a route list 51 according to the first embodiment. [Figure 20] FIG. 10 is a diagram showing an example of the hardware configuration of a control device 1 according to a modified example of the first embodiment. [Figure 21] FIG. 10 is a diagram showing the mechanism of a quadruped walking robot 2A according to a second embodiment. [Figure 22] FIG. 10 is a diagram showing a range in which the position of a foot 265FR according to the second embodiment can be set. [Figure 23] FIG. 10 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the second embodiment. [Figure 24] FIG. 10 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the second embodiment. [Figure 25] FIG. 10 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the second embodiment. [Figure 26] FIG. 10 is a diagram for explaining the processing of a path creation unit 144 according to the second embodiment. [Figure 27] 10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the second embodiment, where (a) is a table corresponding to the initial state, (b) is a table corresponding to the end of the first iteration, and (c) is a table corresponding to the end of the second iteration. [Figure 28] 10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the second embodiment, where FIG. 10A is a table corresponding to the end of the third iteration, and FIG. 10B is a table corresponding to the end of the fourth iteration. [Figure 29] 10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the second embodiment, where FIG. 10A is a table corresponding to the end of the fifth iteration, and FIG. 10B is a table corresponding to the end of the sixth iteration. [Figure 30] 10A and 10B are diagrams illustrating the processing of the path creation unit 144 according to the second embodiment, where FIG. 10A is a table corresponding to the end of the seventh iteration, and FIG. 10B is a table corresponding to the end of the eighth iteration. [Figure 31] FIG. 10 is a diagram for explaining the processing of a path creation unit 144 according to the second embodiment. [Figure 32]10A and 10B are diagrams showing route lists according to the second embodiment, in which (a) is a table showing a route list 52, and (b) is a table showing a route list 53. [Figure 33] FIG. 11 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the third embodiment. [Figure 34] FIG. 11 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the third embodiment. [Figure 35] 11 is a flowchart showing the operations of a connection relationship registration unit 143 and a path creation unit 144 according to the third embodiment. DETAILED DESCRIPTION OF THE INVENTION

[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 3D data while taking into account the specifications of a robot for which a path is to be generated, creates an environmental map showing the connections between the planes based on the results of determining whether or not each of the extracted planes is movable between neighboring planes, and creates a path from the robot's movement start position to its movement destination position based on the connections shown in the created environmental map. The environmental map is made up of a 3D point cloud measured in a target area. As an example of this embodiment, a control device equipped with a 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 apparatus 2 as a movement start position from the robot apparatus 2 via a communication network, and transmits information indicating at least the path calculated by the path generation program 14 to the robot apparatus 2. The communication unit 12 may transmit and receive information to and from other robot apparatuses (not shown) and terminals such as smartphones.

[0015] The memory unit 13 stores three-dimensional global point cloud data 31, which is three-dimensional data corresponding to the area in which the robot 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 three-dimensional global point cloud data 31 .

[0016] The route generation program 14 is made up of a program input / output unit 141, a map creation unit 142, a connection relationship registration unit 143, and a route creation unit 144. Each of the map creation unit 142, the connection relationship registration unit 143, and the route creation unit 144 corresponds to a calculation unit. When the communication unit 12 receives information indicating a movement target position from the user input / output unit 11 or a terminal such as a smartphone (not shown), the communication unit 12 receives information indicating the current position transmitted by the robot apparatus 2 to the communication network. Thereafter, the path generation program 14 generates a movement path for the robot apparatus 2, using the current position indicated by the received information as the movement start position. The information indicating the generated movement path is transmitted to the robot apparatus 2 by the communication unit 12. In this specification, the movement path may 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 converted results.

[0018] When a plane included in a target area is divided into a plurality of grids, the map creation unit 142 sets 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 3D point cloud corresponding to the planar area exists within 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 defines a planar area extracted from each grid from which planar areas are extracted as a target plane, and classifies the target plane as an obstacle plane if a 3D point cloud exists in the grid containing the target plane between the height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of the target moving object. If the map creation unit 142 does not classify the target plane as an obstacle plane, it may classify the target plane as a movement candidate plane. A specific example of the target moving object is the robot device 2. The target moving object may also be a robot device that moves by tires. A specific example of the height direction relative to the target plane is a direction perpendicular to the target plane. The height of the target moving object is the total height of the target moving object. The map creation unit 142 creates a planar map showing at least a part of the extracted planar area. As a specific example, the map creation unit 142 creates a planar map showing, from the extracted planar areas, those planar areas that are not classified as obstacle planes. The map creation unit 142 may include, in the planar map, information indicating the height of each planar area shown on the planar map. The map creation unit 142 may include, in the planar map, each planar area classified as an obstacle planar area, and may include, in the planar map, information indicating the height of an obstacle in each planar area classified as an obstacle planar area.

[0019] The connection relationship registration unit 143 treats each two adjacent planar areas on the planar map as a target adjacent planar pair, and determines whether the target moving body can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the moving mechanism of the target moving body and the 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. A specific example of the geometric structure between the two planar regions included in the target nearby plane pair is the difference in height between the two planar regions included in the target nearby plane pair. The connection relationship registration unit 143 determines whether the target moving object can reach the other planar region of the target nearby plane pair from one planar region of the target nearby plane pair based on the structure of the movement mechanism of the target moving object and the difference in height between the two planar regions included in the target nearby plane pair. Then, if the target moving object can reach the other planar region of the target nearby plane pair from one planar region of the target nearby plane pair, the connection relationship registration unit 143 determines that the target moving object can traverse between the two planar regions included in 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 3D 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 removing 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, for example, a sensor capable of acquiring the surrounding environment as three-dimensional point cloud data, such as a 3D LiDAR (3-Dimensions Light Detection and Ranging) or a stereo camera. The acquired three-dimensional local point cloud data corresponding to the surrounding environment of the robot apparatus 2 is used in the control program 27 for self-position estimation, surrounding environment detection, etc.

[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). The data indicating the rotation speed is mainly used for mechanical 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 a control program 27 .

[0027] The movement mechanism 26 is formed of, for example, tires, and transmits the driving force received from the actuator 25 to the ground in contact with the movement mechanism 26, thereby enabling the robot device 2 to accelerate or decelerate. The movement mechanism 26 may also have a suspension mechanism (not shown) or the like. As a specific example, there are two pairs of actuators 25 and moving mechanisms 26 in the robot device 2. 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 sent 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 3D point cloud sensor 23 acquires 3D local point cloud data, the self-position estimation unit 272 calculates the current position of the robot device 2 by matching the acquired 3D local point cloud data with the 3D global point cloud data stored in the memory unit 22 using the 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 three-dimensional local point cloud data used by the self-position estimation unit 272 in matching that does not match the three-dimensional global point cloud data. In addition, the surrounding environment detection unit 273 may classify at least a portion of the three-dimensional local point cloud data into pedestrians, bicycles, etc. using a deep learning method or the like, and consider the point cloud data classified into pedestrians, bicycles, etc. as data corresponding to temporary obstacles.

[0032] The path following calculation unit 274 calculates a 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 observation 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 be equipped with 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 equipped with hardware such as a processor 101, a memory 102, an auxiliary storage device 103, a display interface 104, a communication interface 105, and an input interface 106. These pieces of hardware are appropriately connected via signal lines. 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, specifically a random access memory (RAM). The memory 102 is also called a primary storage device or a main memory. Data stored in the memory 102 is saved in the secondary storage device 103 as 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 integrated into one unit.

[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 the processor 101 can process. The communication interface 105 is, for example, a communication chip or a network interface card (NIC).

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

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

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

[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 operation procedures of the devices included in the route generation system 90 are collectively referred to as a route generation method. Also, the programs that realize the operations of the devices included in the route generation system 90 are collectively referred to as a route generation program. The flow of how each calculation unit of the path generation program 14 generates a path from the three-dimensional global point cloud data 31 will be explained below with reference to the drawings.

[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 moving plane of the robot apparatus 2. That is, the environmental map is divided into a plurality of grids so that the plane included in the target area is divided. When the robot device 2 is a wheeled robot, it is desirable that the length of one side of the grid be equal to or greater than the total length or width of the robot device 2 so that the robot device 2 fits within the grid. In other words, 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 total length or width of the target moving object.

[0049] 5 is a schematic diagram showing an example of a grid division method, in which three-dimensional global point cloud data 31 is divided into square grids with a side length of L. 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 at the bottom left, (k+1, l) and (k+2, l) are assigned to the grids adjacent to the grid located at the bottom left in the right direction, and (k, l+1) and (k, l+2) are assigned to the grids adjacent to the grid located at the bottom left in the depth direction. For simplicity's sake, the point clouds of the 3D global point cloud data 31 (not shown) are assumed to exist only in the dotted areas, i.e., the road surface and the tree surfaces. Also, out of the total 25 grids in Figure 5, three types of grids 32 to 34, each with a different distribution of point clouds, are shown as examples. For the convenience of the following explanation of the map creation unit 142, the 3D point cloud data is illustrated as being divided into two types: 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 3D 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 a number of times, the number of times being equal to the number of grids generated in step S310. The repeated processes may be executed in parallel. The map creation unit 142 selects a grid that has not yet been selected in the repeated processes as a target grid. The processing from step S330 to step S360 corresponds to a plane extraction process for extracting a plane from each grid.

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

[0052] (Step S340) The map creation unit 142 extracts a plane into which the robot device 2 can enter from the group of points present in the target grid. If one or more planes are extracted from the target grid, 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 showing an example of a plane extraction method for extracting planes into which the robot apparatus 2 can enter using a histogram. First, the map creation unit 142 creates a histogram from the number of points contained in each rectangular parallelepiped obtained by dividing the target grid at 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 desirable that the width of the bin be set equal to or less than the upper limit of the height of a step that the robot device 2 can traverse. This is because, in the obstacle extraction process described below, if a bin above a bin whose plane has been extracted contains 3D point cloud data, the bin above that bin will be determined to be an obstacle. The upper limit of the height of a step that the robot device 2 can traverse may be a value obtained by multiplying the tire diameter of the moving mechanism 26 by a certain coefficient, or may be an actual measurement value obtained by changing the height of the step in stages and having the robot device 2 actually traverse the step. In Figure 6, the threshold for the number of bins determined to be plane candidates is set to 5. The bins determined to be plane candidates in grid 32 have a relatively small standard deviation because they contain only road surface reflection point clouds. On the other hand, the bins determined to be plane candidates in grid 33 have a relatively large standard deviation because they contain both road surface reflection point clouds and tree reflection point clouds.

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

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

[0056] (Step S360) The map creation unit 142 calculates a virtual plane based on the plane information of the adjacent grids 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 lattices located to the front, rear, left, and right of lattice 33. In FIG. 7, the standard deviation threshold in step S340 is set between the standard deviation calculated for lattice 32 and the standard deviation calculated for lattice 33. Therefore, it is assumed that lattice 32 is determined to include a plane, and lattice 33 is determined not to include a plane. Here, when a grid contains a plane, such as grid 32 adjacent to grid 33 on the right, the average height of each plane included in the grid is calculated. There is one plane in each grid adjacent to grid 33 on the front, back, left, and right, and the average height calculated for the planes included in each grid is expressed as h f and h b and h l and h r Let's say. 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. c,img Let h be the average height of one or more selected planes. In Figure 7, all four planes are selected, i.e., h c,img =(h f +h b +h l +h r ) / 4. As a specific example, the predetermined threshold is a threshold set to approximately the width of the bins of the histogram used in step S340. Although there is at most one plane in each grid in FIG. 7, even if there are multiple planes in each grid, the map creation unit 142 sets a threshold value for 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. 3 and 4, the obstacle extraction process is divided into two parts: a repetitive process for the detected plane from step S370 to step S410, and a repetitive process for the virtual plane from step S420 to step S450. However, the difference between these two repetitive processes is whether or not the process corresponding to "registering the plane as a candidate plane for movement" in step S400 is performed, and the rest of the process is essentially the same. Therefore, steps S370 to S410, which are the repetitive process for the detected plane, will be described.

[0059] (Step S370) The processing from step S370 to step S410 is a repetitive processing that is executed the number of times equal to 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 according to the minimum height required for the three-dimensional area in which the robot apparatus 2 can move.

[0061] FIG. 8 is a schematic diagram showing an example of a flow for determining the height for obstacle determination using a histogram. In FIG. 8, it is assumed that in step S340, the standard deviation threshold is set to be equal to or greater than the standard deviation calculated for grid 33, and thus grids 32 and 33 are each determined to contain a plane. For the histogram created in step S340, the map creation unit 142 determines the bin that is closest to the bin determined to include a plane and has one or more bins among the bins located above the bin determined to include a plane as the lowest-point bin.The map creation unit 142 then determines the lowest point among the 3D point cloud included in the lowest-point bin as the obstacle lowest point, and calculates the difference between the height of the detected plane and the height of the obstacle lowest point as the obstacle height minimum value. If the minimum obstacle height calculated as in the example of the grid 32 shown in FIG. 8 is equal to or greater than the movable height threshold set to be equal to or greater than the height of the robot apparatus 2, the map creation unit 142 determines that there is no obstacle and 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 grid 33 shown in Fig. 8. Then, the map creation unit 142 searches each bin located above the bin containing the lowest obstacle point, in order from bottom to top, and determines the 3D point cloud data at the highest position among the 3D point clouds contained in the bin immediately below the bin with zero bins as the highest obstacle point. Then, the map creation unit 142 calculates the difference between the height of the detection plane and the height of the highest obstacle point as the maximum obstacle height value, and proceeds to step S390. Note that the width of the bin is set taking into consideration the traversal capability of the robot device 2, so it is not necessary to extract only road surface reflection point clouds that are strictly flat. Therefore, as in the example of the grid 33 shown in Figure 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 movement planes and obstacle planes in a list format in the plane list 41. The plane list 41 corresponds to a planar map. First, the map creation unit 142 assigns i+4 to the grid 33 determined in step S380 to have an obstacle, as an integer ID (Identifier) ​​that does not overlap with other planes. Next, the map creation unit 142 registers (x, y) = (k+2, l+2) as "coordinate information" in the plane list 41, the "obstacle plane" as the "judgment plane", and the "plane height" and "maximum obstacle height" as numerical information.

[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] In the examples shown in FIGS. 9 and 10, first, the map creation unit 142 assigns i+7 to the grid 32 determined to have no obstacles in step S380 as an integer ID that does not overlap with other planes. Next, the map creation unit 142 registers (x, y) = (k+3, l+2) as "coordinate information," the "candidate plane for movement" as "determination plane," and only the "plane height" as numerical information in the plane list 41. Note that, because there are no obstacles 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 point of each iteration in the repeating process that started in step S370. The map creation unit 142 transitions to step S460 when processing has been completed for all detection planes, and transitions to step S370 when processing has not been completed for all detection planes. Regarding the repeated processing on the virtual plane from step S420 to step S450, whose description has been omitted, if the detected plane is replaced with the virtual plane, step S370 corresponds to step S420, step S380 corresponds to step S430, step S390 corresponds to step S440, and step S410 corresponds to step S450.

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

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

[0069] A method for registering a connection relationship when the traversable threshold is 0.25 m will be described using the candidate travel plane with ID=10 shown in Figures 11 and 12 as an example. In the description of this example, the candidate travel plane with ID=10 is taken as the target plane, and the grid containing the target plane is taken as the target grid. The grid adjacent to the target grid in the x+1 direction has a candidate plane for movement with ID=11 as an adjacent plane. Here, the difference in height between the target grid and the adjacent plane is 0.4 m. Because this difference in height exceeds the traversable threshold, it is determined that movement from the target grid to the adjacent plane is not possible, and the connection relationship between the target grid and the adjacent plane is not registered in the extended plane list 42. Since no plane exists in the lattice adjacent to the target lattice in the x-1 direction, no connection relationship is registered in the extended plane list 42 between the two lattices. The grid adjacent to the target grid in the y+1 direction has a candidate plane for movement with ID=15 as an adjacent plane. Here, the difference in height between the target grid and the adjacent plane is 0.8 m. Because this difference in height exceeds the traversable threshold, it is determined that movement from the target grid to the adjacent plane is not possible, and the connection relationship between the target grid and the adjacent plane is not registered in the extended plane list 42. The grid adjacent to the target grid in the y-1 direction has a candidate plane for movement with ID=7 as an adjacent plane. Here, the difference in height between the target grid and the adjacent plane is 0 m. Because this difference in height is less than the traversable threshold, it is determined that movement from the target grid to the adjacent plane is possible, and the connection relationship between the target grid and the adjacent plane is registered in the extended plane list 42. The connection relationship registration unit 143 creates an extended plane list 42 by performing a process of appropriately registering connection relationships for all of the movement candidate planes 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 described with reference to FIGS. This flowchart was created based on an existing algorithm called the A* (Aster) method.

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

[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. 16 to 18 show the transitions of the unsearched list and the searched list from the start to the end of the process. Both the unsearched list and the searched list must hold at least three pieces of information: ID, parent ID, and cost. For ease of understanding, we will assume that both the unsearched list and the searched list also hold coordinate information. The parent ID is the ID of the plane one level back on the route. However, since there is no such plane at the start position of the movement, the ID of the start position of the movement is set as the parent ID corresponding to the start position of the movement. If the cost decreases as the user approaches the destination position, it is possible to generate a route to reach the destination position based on the cost. There are several methods for calculating such a cost, but in this embodiment, we use [Formula 1] as the formula for calculating the cost of the target ID.

[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 grids corresponding to each of the two IDs. For the ID whose cost is to be calculated in the initial unsearched list, the parent ID is the target ID (own ID), so the cost of the parent ID, C(PID) = 0. Also, since M(ID, GID) = M(PID, GID), M(ID, GID) and M(PID, GID) cancel each other out. Therefore, C(1) = 1.

[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 proceeds 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." In the initial state, the route generation 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 route 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 order of cost in step S540, the route 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 generation 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 path generation unit 144 proceeds to step S570, and otherwise proceeds 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 generation 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 combined ID: CID=2,5.

[0081] (Step S580) The process from step S580 to step S630 is a repetitive process that is executed the number of times equal to the number of bond IDs acquired in step S570. The path creation unit 144 selects a bond ID that has not yet been selected in the repetitive process as a target bond 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 connection IDs CID=2, 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 see if there is a row having the same ID as the target link ID.

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

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

[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 the connection IDs, and transitions to step S580 when processing has not been completed for all the connection 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. 16(b) shows the unsearched list and the searched list at the end of the first iteration of step S640. The row with the current search ID: NID=1 has been moved to the searched list, and the rows with the combined IDs CID=2 and CID=5 have been added to the unsearched list.

[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 has the second priority. In step S570, the combined ID: CID=1,9. In the repeated process for each bond ID starting from step S580, if the bond ID: CID=1, the cost C(1) calculated in step S590 is 3, and ID=1 is in the searched list. Therefore, a branch to step S650 occurs in step S610.

[0089] (Step S650) The path creation unit 144 checks whether the unsearched list contains the same ID 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 proceeds 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 proceeds to step S670. Since ID=1 exists in the searched list, the route creation unit 144 proceeds to step S670. Note that the list updates in steps S660 and S670 are the same process except for the lists being updated, so 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 has the same target link ID as the target link ID in the searched list. If the most recently calculated cost is smaller in step S590, the path creation unit 144 updates the "parent ID" of the row 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 the rows within the list are not moved.

[0091] For ID=1, the most recently calculated cost C(1) = 3, and the cost of the searched list C(1) = 1. Therefore, the searched list is not updated, and the row with ID=1 is not moved. For CID=9, the cost C(9)=3, and CID=9 does not exist in either list, so the row for CID=9 is listed in 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, the cost is the same between IDs 2 and 9, so the current search ID: NID=9 is given the second priority. In step S570, the combined ID: CID=5,12 is set. For CID=5, the cost C(5)=3, so the cost indicated by the searched list is smaller, so the list is not updated. For CID=12, the cost C(12)=3, and the cost C(12) is listed 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 path creation unit 144 starts a search again from the ID where the parent ID: PID=1. The remaining repeated processes are omitted, but in the seventh iteration of 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 searched list from the movement target position to the movement start position in order of parent IDs, and ends the processing of this flowchart with the result "arrival completed." Specifically, the parent ID corresponding to the current search ID: NID=10 is PID=7, and the parent ID of the row with ID=7 in the searched list is PID=3, and so on. By tracing the parent IDs in this order, it is possible to obtain coordinate information for the route connecting the movement start position to the movement target position, which is ID=1→2→3→7→10.

[0095] Here, it is possible to control the robot device 2 with respect to the route using only the coordinate information of the route. However, in controlling the robot device 2 with respect to the route, it is common to use speed information at each position on the route. 19 shows a route list 51 to which speed information has been added. The route list 51 corresponds to (b) of FIG. 18. The route creation unit 144 may generate the route list 51. In the speed information, only the speed corresponding to the movement target position may be set to zero, and the speeds corresponding to all other positions on the 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 provide 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 absolute upper limit of the speed change for moving one space between grids 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 route generation program 14 is included in the control device 1, but the robot device 2 may execute the processing of the route generation program 14 by using the three-dimensional global point cloud data included in the memory unit 22. Moreover, in the first embodiment, only the control device 1 and one robot device 2 are described. However, the control device 1 may generate routes 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 effect of the first embodiment*** 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 the three-dimensional area through which the autonomous mobile robot passes while moving, it is not determined that an obstacle exists within the three-dimensional area. This eliminates the need to impose restrictions on sensor placement and enables the autonomous mobile robot to move indoors, under bridge girders, under tree branches, etc.

[0098] ***Other Configurations*** <Variation 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 in place 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 each unit that the control device 1 has. The processing circuitry 111 may be dedicated hardware, or may be a processor that executes a program stored in the memory 102 .

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

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

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

[0103] ***Configuration Description*** In the first embodiment, it is assumed that the robot apparatus 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 a traversable threshold. Here, when the robot apparatus 2 has a movement mechanism with legs, such as a bipedal or quadrupedal movement mechanism, not only is the possibility of traversing between adjacent candidate movement planes improved, but even when the two candidate movement planes are not adjacent to each other, the robot apparatus 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 a 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 a plurality of legs as a movement mechanism of the target moving object.

[0105] The route creation unit 144 according to this embodiment creates a route by preferentially selecting, from among the registered connection relationships, connection relationships with a relatively low corresponding travel difficulty. The route creation unit 144 may set the speed of a target moving object corresponding to a connection relationship with a relatively high corresponding travel difficulty to be lower than the speed of a target moving object corresponding to a connection relationship with a relatively low corresponding travel difficulty. Note that, between two planar regions corresponding to each registered connection relationship, a travel 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), the front left leg (FL), the rear right leg (RR), and the rear left leg (RL) are respectively connected to the torso link 261 via the first motor 251FR, the first motor 251FL, the first motor 251RR, and the first motor 251RL so that the axes are rotatable parallel to the X axis of the torso link 261. The first link 262FR, the first link 262FL, the first link 262RR, and the first link 262RL are fixedly connected to each first motor 251 so that the longitudinal direction of each first link 262 faces the radial direction of the rotation axis of the first motor 251FR, the first motor 251FL, the first motor 251RR, and the first motor 251RL. The rotating shafts of second motor 252FR, second motor 252FL, second motor 252RR, and second motor 252RL are connected to each first link 262 so as to be parallel to the longitudinal direction of first link 262FR, first link 262FL, first link 262RR, and first link 262RL, and so as to be rotatable. The second link 263FR, the second link 263FL, the second link 263RR, and the second link 263RL are each connected to each second motor 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 link 263RL, and so that they are rotatable. The third motors 253FR, 253FL, 253RR, and 253RL are connected to the second links 263FR, 263FL, 263RR, and 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. The third link 264FR, the third link 264FL, the third link 264RR, and the third link 264RL are each connected to each third motor 253 so that the longitudinal direction of each third link 264 faces the radial direction of the rotation axis of the third motor 253FR, the third motor 253FL, the third motor 253RR, and the third motor 253RL. The foot portion 265FR, the foot portion 265FL, the foot portion 265RR, and the foot portion 265RL are fixedly connected to each third link 264 at one end in the longitudinal direction of the third link 264FR, the third link 264FL, the third link 264RR, and the third link 264RL, respectively. The first motor 251 and the second motor 252 each correspond to a hip joint of the animal, and the third motor 253 corresponds to a knee joint of the animal. A rotation sensor, a torque sensor, and the like (not shown) are attached to each of the first motor 251, the second motor 252, and the third motor 253. The robot device 2 calculates and controls the state of the leg, such as the posture, based on the information acquired by these sensors. For each leg, the lengths of each first link 262, each second link 263, and each third link 264 (including the foot 265) 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 the quadruped walking robot 2A in static walking, in which each leg is lifted off the ground as a swing leg, is calculated. As a specific example, consider a case where 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 in the front right leg is taken as the origin, when the joint angles of the first motor 251FR, the second motor 252FR, and the third motor 253FR are θ1, θ2, and θ3, respectively, the three-dimensional coordinate (X FR ,Y FR ,Z FR ) is as shown in [Equation 1]. Note that C k =cosθ k , S k =sinθ k (k=1,2,3).

[0108]

number

[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. If θ1, θ2, and θ3 are not limited, the position of the foot 265FR can be set within the range of an ellipsoid 28 centered at the origin. However, in reality, there are limits to 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 not appropriate, and the other legs are in a swing position, this can cause the quadruped walking robot 2A to lose balance or slip. The range of possible optimal positions and postures of the foot 265FR when it touches the ground is calculated taking into consideration various parameters such as the inclination of the road surface. However, the inclination of the road surface can be limited by the conditions of 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. In the foot setting cube 29, a range of plane symmetry is set with respect to the YZ plane in the direction of travel (X direction in the figure) so that the quadruped walking 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 motion 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 so 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. From the above, it is possible that the quadrupedal robot 2A can traverse a candidate movement plane, at least a portion of which is within the range of the foot setting cube 29, if 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 on the ground is within the range of the foot setting cube 29.

[0110] For each grid for creating the candidate movement plane and the obstacle plane, 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 and the total width of the robot device 2 so that the robot device 2 can fit within each grid. However, for the quadruped walking robot 2A, it is sufficient that the feet 265 can stably touch the ground, so it is desirable that the length of one side of each grid be approximately equal to the greater of the length and the width of the bottom surface of the feet 265, plus a margin of several times. Here, because 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 plane 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, and further 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 augmented plane list 43 created by modifying the augmented plane list 42 for the quadruped walking robot 2A. The modified augmented plane list 43 is shown divided into two figures. The modified augmented plane list 43 corresponds to a planar map. Figure 25 shows a schematic diagram corresponding to the modified extended plane list 43 shown in Figures 23 and 24. Figure 25 shows connection relationships common to wheeled robots and quadruped robots, as well as connection relationships that apply only to quadruped robots. In this example, the obstacle plane corresponding to ID=6 is divided into four planes corresponding to ID=17 to 20. Of the divided planes, the planes corresponding to ID=17 and 20 are determined to be candidate planes for movement. The connection relationships of the quadruped walking robot 2A share some commonalities with the connection relationships of the wheeled robot. In addition, the traversal threshold for the wheeled robot 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, a new connection has been made between ID=10 and ID=11. 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, and the height difference between the corresponding planes for each of these ID pairs is less than or equal to the traversal threshold, so these ID pairs are in a connected relationship. For each of ID=2 and ID=20, and ID=17 and ID=7, it is determined that the right leg (both front and back) can get over it, and these ID pairs are in a connected relationship. For ID=5 and ID=20, it is determined that the left leg (both front and back) can get over it, and this ID pair is in a connected relationship. In addition, when 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] In registering the connection relationship for the quadruped walking robot 2A, the connection relationship may be registered when only one of the right leg and the left leg is capable of climbing over. As a specific example, when the quadrupedal walking robot 2A has its front right leg placed on a movement candidate plane corresponding to a certain ID that has been determined to have a connection relationship with 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 walking robot 2A is the front left leg. Therefore, if there are movement candidate planes on which the quadrupedal walking robot 2A can move alternately using the left and right feet, or on which the quadrupedal walking robot 2A can move using either the left or right leg, depending on the state of the legs of the quadrupedal walking 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. 27 to 31 show the transition of the unsearched list and searched list from the start to the end of the search process taking into account the difficulty of movement. In the first embodiment, cost calculation was performed taking into consideration only Manhattan distance. However, in some cases, smoother movement can 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 move along. Therefore, in this embodiment, [Formula 2] is adopted as the formula for calculating the cost of the 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 candidate plane in [Formula 1] is replaced with the difficulty of moving between the target ID and parent ID in a connection relationship, D(TID, PID). D(TID, PID) is defined by [Formula 2]

[0116]

number

[0117] Also, IDs 17 to 20 were generated by dividing ID 6. Therefore, M(17, GID) = M(18, GID) = M(19, GID) = M(20, GID). When the route creation unit 144 employs [Formula 2] that takes into account the degree of difficulty of movement, as can be seen from the searched list at the end of the ninth iteration shown in Figure 31, it is possible to obtain coordinate information for the route ID = 1 → 2 → 3 → 7 → 8 → 11, which does not go over any obstacle planes and traverses relatively low steps, connecting the movement start position to the movement target position. On the other hand, if the route generation unit 144 were to use [Formula 1] that does not take into account the travel difficulty, the cost of traveling to ID=7 would be 1, not 1.5, at the end of the fourth iteration of step S640. Therefore, a route that overcomes the obstacle plane would be 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 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] Figure 32(a) shows a specific example of a route list 52 in which speed information is added to the coordinate information of a route when the travel difficulty level shown in Figure 31 is taken into consideration. Figure 32(b) shows a specific example of a route list 53 in which speed information is added to the coordinate information of a route when the travel difficulty level is not taken into consideration. Regarding a route that takes into consideration the degree of difficulty of movement, when the quadruped walking robot 2A does not climb over an obstacle plane and when the quadruped walking robot 2A traverses a relatively low step, it is desirable to provide speed information indicating that the quadruped walking robot 2A 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 so that the speed becomes 0 before the movement target position, as in Embodiment 1. Figure 32(a) shows speed settings when the upper limit of the absolute value 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 travel difficulty into consideration, or for a route that does take travel difficulty into consideration but involves climbing over an obstacle plane or traversing a relatively high step, by setting the maximum speed when climbing over an obstacle plane and the maximum speed when climbing over a high step in addition to the absolute upper limit of speed change, the speed upper limit at corners, and the constraint on the maximum speed, it is possible to reduce the risk of control issues on a route with high travel difficulty, such as a collision with an obstacle on the obstacle plane or failure to climb over a high step. In (b) of FIG. 32, in addition to the speed settings similar to those in the first embodiment, the maximum speed when climbing over an obstacle plane is set to 2 km / h and the maximum speed when climbing over 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 speed change. At ID=5, the quadruped walking robot 2A accelerates to 3 km / h, which is the upper speed limit at corners, because it is a corner. At ID=17, the quadruped walking robot 2A decelerates to 2 km / h, which is the maximum speed when climbing over an obstacle plane, in order to climb over the obstacle plane. At ID=7, the quadruped walking robot 2A decelerates to 3 km / h, which is the upper speed limit for corners. At ID=10, the quadruped walking robot 2A decelerates to 1 km / h because this is the candidate plane for movement just 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 set to constant values, but these maximum speeds may be set to variable maximum speeds, such as maximum speeds 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 the effect of the second embodiment*** 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] Embodiment 3 The following mainly describes the differences from the above-described embodiment with reference to the drawings.

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

[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 or not 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 then classifies all planar areas that can be reached from the first point by tracing each registered connection relationship as non-travelable 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 it is determined that a route for the target moving body from the first point to the second point cannot be created, the route creation unit 144 creates a route from the first point to a point on a planar area that is relatively close to the second point among planar areas that are not classified as obstacle planes and are shown on the planar map.

[0125] ***Explanation of Operation*** FIG. 33 is a schematic diagram showing, in the form of a grid map, an example of information in the plane list 41 including candidate planes for movement that cannot have a connection relationship registered. FIG. 34 shows an expanded plane list 42 corresponding to the grid map shown in FIG. In this example, the movement candidate plane corresponding to ID=12 is found to be an unmovable plane because no connection relationship with other movement candidate planes is registered. On the other hand, movement candidate planes such as ID=13, 14, and 15, which have connection relationships registered but cannot be connected to routes from movement candidate planes other than the movement candidate planes corresponding to these IDs, are also unmovable planes. However, the removal of multiple movement candidate planes isolated from multiple other movement 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 processing from step S710 to step S820 is a repetitive process that is executed the same number of times as the number of rows in the extended plane list 42. In this repetitive process, 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 candidate plane for movement or an obstacle plane. If the target plane is an obstacle plane, the connection relationship registration unit 143 does not perform any processing and moves on to processing the next line in the extended plane list 42.

[0129] (Step S730) The processing in this step is processing for determining whether the target plane is a candidate plane for movement, which has already been determined as either a movable plane or an immovable plane. If the target plane is a candidate plane that has been determined to be a movable plane or an immovable plane, the connection relationship registration unit 143 does not perform any processing and proceeds to processing 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, and 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 the next line in the extended plane list 42.

[0132] (Step S760) 13 and 14, the route creation unit 144 sets a target plane for the movement target position, and sets a movable plane that is closest to the target plane for the movement start position, and executes the route search process shown in Fig. 13 and Fig. 14. 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 is no available plane, one of the candidate planes must be set as the available plane in advance in order to execute the processing of this step. Here, it is appropriate to set the candidate plane that corresponds to the current position of the robot apparatus 2 as the available plane. In addition, when selecting the closest available plane, the path generation unit 144 may select the closest available plane in terms of Manhattan distance ignoring the connection relationship (when there are multiple closest available planes, select any one of the available planes).

[0133] (Step S770) Regardless of whether the path 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 line, 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] (Step S800) 13 and 14, the path creation unit 144 sets a target plane (immovable plane) for the movement start position, and sets 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 unsearched list calculated until the path search process in step S800 ends as "non-reachable," 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 unsearched list as immovable planes. This makes it possible to collectively discover immovable planes associated with one immovable 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 it has completed processing 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 creating an environmental map at the same time as self-localization, like SLAM. When a new candidate plane for movement is created and the augmented plane list 42 is updated, the process of the flowchart is performed, thereby removing candidate planes for movement that cannot be set as a movement target position from the augmented plane list 42. However, when the range of the environmental map is expanded, a certain immovable plane may become a movable plane due to the discovery of a detour. Therefore, when the process shown in Figure 35 is executed again, it is advisable to re-determine all areas previously determined as immovable planes as undetermined candidate planes for movement. However, there is no need to re-determine movable planes. Furthermore, when the robot device 2 is caused to automatically create an environmental map, a route from the current position of the robot device 2 to the movable plane that is closest to the immovable plane can be generated by a route search process similar to that of steps S760 and S800, thereby increasing the possibility of discovering a detour that allows the immovable plane to be re-determined as a movable plane.

[0140] ***Explanation of the effect of the 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 may be freely combined, or any of the components in each embodiment may be modified, or any of the components in each embodiment may be omitted. Furthermore, the embodiments are not limited to those described in the first to third embodiments, and various modifications are possible as necessary. The procedures explained using flowcharts and the like may be modified as appropriate. [Explanation of symbols]

[0142] 1 Control device, 11 User input / output unit, 12 Communication unit, 13 Memory unit, 14 Path generation program, 141 Program input / output unit, 142 Map creation unit, 143 Connection relationship registration unit, 144 Path creation unit, 2 Robot device, 21 Communication unit, 22 Memory unit, 23 3D point cloud sensor, 24 Rotation sensor, 25 Actuator, 26 Moving mechanism, 27 Control program, 271 Program input / output unit, 272 Self-position estimation unit, 273 Surrounding environment detection unit, 274 Path following calculation unit, 275 Mechanism calculation unit, 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 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 such that a plane included in the target area is divided, 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 unit that defines two adjacent planar areas in the planar map as a target adjacent planar pair, 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, 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 does not register the connection relationship of the target adjacent planar pair if the target moving object cannot traverse between the two planar areas included in the target adjacent planar pair; a route creation unit that creates a route of 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; A route generation device comprising:

2. 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 such that a plane included in the target area is divided, 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 unit that defines two adjacent planar areas in the planar map as a target adjacent planar pair, 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; a route creation unit that creates a route of 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; A route generation device comprising: the map creation unit defines 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 when a three-dimensional point cloud exists in a grid including the target plane between a height of the target plane and a height obtained by adding a movable height threshold, the height of the target plane being equal to or greater than the height of the target moving body, in a height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, The connection relationship registration unit determines whether it is possible to create a route 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 route 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-movable planes.

3. 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 such that a plane included in the target area is divided, 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 unit that defines two adjacent planar areas in the planar map as a target adjacent planar pair, 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; a route creation unit that creates a route of 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; A route generation device comprising: the map creation unit defines 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 when a three-dimensional point cloud exists in a grid including the target plane between a height of the target plane and a height obtained by adding a movable height threshold, the height of the target plane being equal to or greater than the height of the target moving body, in a height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the connection relationship registration unit determines whether or not it is possible to create a route for the target moving object from a first point on a planar area not classified as an obstacle plane, which is shown on the planar map, to a second point on a planar area not classified as an obstacle plane, which is shown on the planar map; The route creation unit is a route generation device that, when it is determined that a route for the target moving body from the first point to the second point cannot be created, creates a route from the first point to a point on a planar area that is not classified as an obstacle plane and is shown on the planar map, and that is relatively close to the second point.

4. the map creation unit includes information indicating the height of each planar area shown on the planar map, the geometric structure between the two planar regions included in the target near planar pair includes a height difference between the two planar regions included in the target near planar pair; The path generation device of claim 1, wherein the connection relationship registration unit determines whether 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 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 plane pair, and 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.

5. the map creation unit defines 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 when a three-dimensional point cloud exists in a grid including the target plane between a height of the target plane and a height obtained by adding a movable height threshold, the height of the target plane being equal to or greater than the height of the target moving body, in a height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, 5. The route generation device according to claim 1, wherein the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map.

6. the map creation unit includes each planar area classified as an obstacle plane in the planar map, includes information indicating a height of each planar area shown by the planar map in the planar map, includes information indicating a height of an obstacle in each planar area classified as an obstacle plane in the planar map, The path generation device of claim 5, 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.

7. The path generation device described in claim 5, 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.

8. the connection relationship registration unit determines whether or not it is possible to create a route for the target moving object from a first point on a planar area not classified as an obstacle plane, which is shown on the planar map, to a second point on a planar area not classified as an obstacle plane, which is shown on the planar map; 6. The path generation device according to claim 5, wherein, when it is determined that a path for the target moving body from the first point to the second point cannot be created, the path creation unit creates a path from the first point to a point on a planar area that is not classified as an obstacle plane and is shown on the planar map, and that is relatively close to the second point.

9. a degree of difficulty of movement between two planar regions corresponding to each registered connection relationship is set based on a geometric structure between the two planar regions corresponding to each registered connection relationship; The route generation device according to claim 1 , wherein the route generation unit generates the route by preferentially selecting, from among the registered connection relationships, connection relationships having a relatively low corresponding degree of travel difficulty.

10. a degree of difficulty of movement between two planar regions corresponding to each registered connection relationship is set based on a geometric structure between the two planar regions corresponding to each registered connection relationship; 5. A path generation device according to claim 1, wherein the path creation unit sets the speed of the target moving body corresponding to a connection relationship in which the corresponding movement difficulty is relatively high to a speed that is lower than the speed of the target moving body corresponding to a connection relationship in which the corresponding movement difficulty is relatively low.

11. The path generation device according to claim 1 , wherein the target moving object is a robot device having a plurality of legs as a movement mechanism of the target moving object.

12. When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids such that a plane included in the target area is divided, the computer determines 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 part of the extracted planar area; The computer defines each of 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 does not register the connection relationship of the target adjacent planar pair if the target moving object cannot traverse between the two planar areas included in the target adjacent planar pair, A route generation method in which the computer generates a route 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.

13. When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids such that a plane included in the target area is divided, the computer determines 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 part of the extracted planar area; The computer defines each of 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 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, the computer creates a route 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; the computer defines a planar area extracted from each grid from which a planar area has been extracted as a target plane, and classifies the target plane as an obstacle plane when a three-dimensional point cloud exists in the grid including the target plane between a 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 in a height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, A path generation method in which the computer 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.

14. When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids such that a plane included in the target area is divided, the computer determines 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 part of the extracted planar area; The computer defines each of 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 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, the computer creates a route 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; the computer defines a planar area extracted from each grid from which a planar area has been extracted as a target plane, and classifies the target plane as an obstacle plane when a three-dimensional point cloud exists in the grid including the target plane between a 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 in a height direction relative to the target plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, The computer determines whether or not it is possible to generate a route for the target moving object from a first point on a planar area not classified as an obstacle plane, which is shown on the planar map, to a second point on a planar area not classified as an obstacle plane, which is shown on the planar map; A route generation method in which, when the computer determines that it is unable 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 and is shown on the planar map, and that is relatively close to the second point.

15. a map creation process 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 a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists in the target grid, a planar area is extracted from the target grid, and a planar map showing at least a part of the extracted planar area is created; a connection relationship registration process in which, on the planar map, each of two adjacent planar areas is defined as a target adjacent planar pair, and whether or not the target moving object can traverse between the two planar areas included in the target adjacent planar pair is determined 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, the connection relationship of the target adjacent planar pair is registered, and if the target moving object cannot traverse between the two planar areas included in the target adjacent planar pair, the connection relationship of the target adjacent planar pair is not registered; a route creation process for creating a route of 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; A route generation program that causes a route generation device, which is a computer, to execute the above.

16. a map creation process 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 a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists in the target grid, a planar area is extracted from the target grid, and a planar map showing at least a part of the extracted planar area is created; a connection relationship registration process in which, on the planar map, each of two adjacent planar areas is defined as a target adjacent planar pair, and whether or not the target moving object can traverse between the two planar areas included in the target adjacent planar pair is determined 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, the connection relationship of the target adjacent planar pair is registered; a route creation process for creating a route of 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; A route generation program that causes a route generation device that is a computer to execute the above, In the map creation process, a planar area extracted from each grid from which a planar area is extracted is taken as a target plane, and in a grid including the target plane, if a three-dimensional point cloud exists between a height of the target plane and a height obtained by adding a movable height threshold value, which is equal to or greater than the height of the target moving body, in a height direction relative to the target plane, the target plane is classified as an obstacle plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, In the connection relationship registration process, a path generation program is provided which 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 is determined 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-movable planes.

17. a map creation process 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 a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists in the target grid, a planar area is extracted from the target grid, and a planar map showing at least a part of the extracted planar area is created; a connection relationship registration process in which, on the planar map, each of two adjacent planar areas is defined as a target adjacent planar pair, and whether or not the target moving object can traverse between the two planar areas included in the target adjacent planar pair is determined 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, the connection relationship of the target adjacent planar pair is registered; a route creation process for creating a route of 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; A route generation program that causes a route generation device that is a computer to execute the above, In the map creation process, a planar area extracted from each grid from which a planar area is extracted is taken as a target plane, and in a grid including the target plane, if a three-dimensional point cloud exists between a height of the target plane and a height obtained by adding a movable height threshold value, which is equal to or greater than the height of the target moving body, in a height direction relative to the target plane, the target plane is classified as an obstacle plane; the planar map indicates planar areas that are not classified as obstacle planes; Each planar region of the target near-plane pair is a planar region that is not classified as an obstacle plane, the movement start position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, the movement target position is a position on a planar area that is not classified as an obstacle plane and is shown on the planar map, In the connection relationship registration process, it is determined whether or not it is possible to create a route for the target moving object from a first point on a planar area not classified as an obstacle plane, which is shown on the planar map, to a second point on a planar area not classified as an obstacle plane, which is shown on the planar map; In the route creation process, if it is determined that a route for the target moving body from the first point to the second point cannot be created, the route generation program creates a route from the first point to a point on a planar area that is not classified as an obstacle plane and is shown on the planar map, and that is relatively close to the second point.

Images