MOBILE BODY, MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND MOBILE BODY CONTROL PROGRAM

Abstract

The mobile body 100 comprises a mobile body 1, a sensor 3 that detects objects around the mobile body 1, and a control device 6 that causes the mobile body 1 to move autonomously while estimating the self-position of the mobile body 1 based on the detection signal of the sensor 3. The control device 6 acquires three-dimensional point cloud data indicating the position information of structures around the mobile body 1 from the detection signal of the sensor 3, extracts at least one point from each of the multiple points included in the three-dimensional point cloud data of the structures, classifies the extracted points into corresponding areas among the three-directional areas in the coordinate system of the mobile body 1 until the number of points included in each area reaches a specified number, and compares the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body 1 moves to estimate the self-position of the mobile body 1.

Description

[Technical Field]

[0001] The technology disclosed herein relates to a mobile body, a control device for a mobile body, a control method for a mobile body, and a control program for a mobile body. [Background technology]

[0002] BACKGROUND ART Autonomous moving bodies have been known for some time. For example, Patent Document 1 discloses a moving body that moves autonomously while estimating its own position. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2014-195868 Summary of the Invention

[0004] In the mobile object described above, sensors are used to detect structures such as walls and floors around the mobile object, and the mobile object estimates its own position based on the detection signals of the sensors. The mobile object acquires point cloud data indicating the position information of the structures from the detection signals of the sensors.

[0005] However, sensors may be unable to detect structures due to obstacles near the mobile body. In this case, depending on the location of the obstacle, there may be an extremely small amount of point cloud data for the structure in a specific direction among the three directions in the coordinate system of the mobile body. In this case, when the point cloud data is compared with map data to estimate the self-location, an imbalance occurs in the scan matching between the point cloud data and the map data in the three directions of the mobile body, reducing the accuracy of the self-location estimation.

[0006] The technology disclosed herein has been developed in consideration of these points, and its purpose is to reduce the deterioration in accuracy of self-position estimation by using simple comparison operations without increasing the computational load.

[0007] The mobile body disclosed herein comprises a mobile body, a sensor that detects objects around the mobile body, and a control device that causes the mobile body to move autonomously while estimating the self-position of the mobile body based on the detection signal of the sensor, wherein the control device acquires three-dimensional point cloud data indicating the position information of structures around the mobile body from the detection signal of the sensor, extracts at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, classifies the extracted points into corresponding areas among three directional areas in the coordinate system of the mobile body until the number of points included in each area reaches a specified number, and compares the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body moves to estimate the self-position of the mobile body.

[0008] The mobile body control device disclosed herein is a mobile body control device that causes the mobile body to perform autonomous movement while estimating the self-position of the mobile body, and is equipped with an acquirer that acquires three-dimensional point cloud data indicating the position information of structures surrounding the mobile body, a classifier that extracts at least one point from a plurality of points included in the three-dimensional point cloud data of the structure and classifies the extracted points into corresponding areas among three directional areas in the coordinate system of the mobile body until the number of points included in each area reaches a specified number, and a comparator that compares the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body moves to estimate the self-position of the mobile body.

[0009] The method for controlling a moving body disclosed herein is a method for controlling a moving body that causes the moving body to perform autonomous movement while estimating the self-position of the moving body, and includes the steps of: acquiring three-dimensional point cloud data indicating the position information of structures surrounding the moving body; extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure; classifying the extracted points into corresponding areas among three directional areas in the coordinate system of the moving body until the number of points included in each area reaches a specified number; and comparing the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the moving body moves, to estimate the self-position of the moving body.

[0010] The control program for a mobile body disclosed herein is a control program for a mobile body for causing the mobile body to perform autonomous movement while estimating the self-position of the mobile body, and causes a computer to realize the following functions: a function for acquiring three-dimensional point cloud data indicating the position information of structures surrounding the mobile body; a function for extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure and classifying the extracted points into corresponding areas among three directional areas in the coordinate system of the mobile body until the number of points included in each area reaches a specified number; and a function for estimating the self-position of the mobile body by comparing the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body moves.

[0011] According to the mobile body, the control device for the mobile body, the control method for the mobile body, and the control program for the mobile body, a decrease in the accuracy of self-position estimation can be reduced without increasing the calculation load by using a simple comparison operation. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 1 is a perspective view of a moving object. [Figure 2] FIG. 2 is a schematic diagram showing the detection range of the sensor. [Figure 3] FIG. 3 is a diagram illustrating a hardware configuration of the control device. [Figure 4] FIG. 4 is a functional block diagram showing the configuration of the control system of the processor. [Figure 5] FIG. 5 is a flowchart of the basic operation of a mobile unit. [Figure 6] FIG. 6 is a detailed functional block diagram of the state estimator. [Figure 7] FIG. 7 is a schematic plan view showing the relationship between the first and second regions of the moving body and points included in the three-dimensional point cloud data of the wall. [Figure 8] FIG. 8 is a schematic diagram illustrating another method for classifying extracted points into the first region or the second region of the mobile body. [Figure 9] FIG. 9 is a flowchart showing the self-position estimation process performed by the state estimator. [Figure 10] FIG. 10 is a flowchart of a subroutine of the acquisition process. [Figure 11] FIG. 11 is a flowchart of the classification processing subroutine. [Figure 12] FIG. 12 is a schematic diagram illustrating the classification process by the classifier. [Figure 13] FIG. 13 is a functional block diagram showing the configuration of a control system of a processor according to a modified example. [Figure 14] FIG. 14 is a side view of the mobile body when the robot arm is in the running configuration. [Figure 15] FIG. 15 is a plan view of the mobile body when the robot arm is in the running configuration. DETAILED DESCRIPTION OF THE INVENTION

[0013] Exemplary embodiments will be described in detail below with reference to the drawings. FIG. 1 is a perspective view of a moving body 100. The moving body 100 moves autonomously. The moving body 100 includes a moving body main body 1 and a control device 6 that causes the moving body main body 1 to move autonomously. For example, the moving body 100 moves within a facility such as a store, hospital, or nursing home. In addition to moving, the moving body 100 may also perform tasks such as handing over an item or opening and closing a door.

[0014] For example, the mobile body 1 is a mobile robot that includes a robot arm 12. In detail, the mobile body 1 may have a carriage 10, a base 11 mounted on the carriage 10, and a robot arm 12 connected to the base 11.

[0015] The bogie 10 has a defined front-to-rear direction. In this example, the bogie 10 has a generally rectangular planar shape. For example, the longitudinal direction of the rectangle is the front-to-rear direction. The lateral direction of the rectangle is the left-to-right direction.

[0016] The dolly 10 includes a plurality of wheels 13 and is capable of traveling. In this example, the dolly 10 includes four wheels 13. The dolly 10 may be capable of traveling straight and turning. In this example, the dolly 10 is capable of moving forward, backward, left, right, and diagonally while maintaining its posture, i.e., moving in all directions. In other words, the dolly 10 may be capable of translational movement in directions other than the forward and backward direction. Furthermore, the dolly 10 may also be capable of rotating on the spot. For example, the four wheels 13 include a set of wheels 13 aligned in the left-right direction at the front of the bottom of the dolly 10 and another set of wheels 13 aligned in the left-right direction at the rear of the bottom of the dolly 10. The wheels 13 may be arranged to form a rectangle on the bottom of the dolly 10. More specifically, the four wheels 13 are arranged at the four corners of the bottom of the dolly 10.

[0017] Specifically, the wheel 13 may be an omnidirectional wheel. In this example, the wheel 13 is a Mecanum wheel. The wheel 13 has a plurality of barrel-shaped rollers arranged around the outer periphery of the wheel. For example, the rotation axis of each roller is inclined at 45 degrees relative to the axle of the wheel 13.

[0018] The mobile body 1 may have a motor 13a that drives the wheels 13 and an encoder 13b that detects the amount of rotation of the motor 13a (see FIG. 3). In this example, the mobile body 1 has four sets of motors 13a and encoders 13b corresponding to the four wheels 13. The four wheels 13 may be independently driven by the corresponding motors 13a.

[0019] The cart 10 may be able to move in any direction in two dimensions using these four wheels 13. For example, the cart 10 can translate or turn in any direction, including forward / backward, left / right, and diagonal. The cart 10 can also rotate on the spot.

[0020] The base 11 may be mounted on the cart 10. In this example, the base 11 has a shape resembling the upper half of a human body. The base 11 may be fixed to the cart 10 so as not to be movable.

[0021] The mobile body 1 has two robot arms 12. A hand 14 may be attached to the tip of one of the robot arms 12. The other robot arm 12 does not necessarily have to have a hand 14 attached to the tip.

[0022] The two robot arms 12 are connected to different portions of the base 11. For example, the two robot arms 12 are connected to different portions of the base 11 in the width direction, which is one direction in a plan view. In other words, the width direction is the direction in which the connection portion of one robot arm 12 to the base 11 and the connection portion of the other robot arm 12 to the base 11 are aligned in a plan view. The base 11 may have a front and a back that face opposite each other in a plan view. For example, the front side of the base 11 is the front, and the back side is the rear, defining the front-to-rear direction. The width direction may be a horizontal direction that is perpendicular to the front-to-rear direction. In other words, the width direction is the left-to-right direction relative to the front-to-rear direction. For example, one robot arm 12 is connected to the left side of the base 11, and the other robot arm 12 is connected to the right side of the base 11.

[0023] When the planar shape of the carriage 10 is a substantially rectangular shape having a longitudinal direction and a lateral direction, the width direction substantially coincides with the lateral direction of the planar shape of the carriage 10.

[0024] For example, as shown in FIG. 1, the robot arm 12 has a plurality of links L and a plurality of joints J that connect the links L. The robot arm 12 is configured to operate in three dimensions. In this example, the robot arm 12 is a multi-joint robot arm. That is, the shape of the robot arm 12 may be freely changed by rotating the joints. The robot arm 12 is supported by a base 11.

[0025] For example, the multiple links L include a first link L1, a second link L2, a third link L3, a fourth link L4, a fifth link L5, a sixth link L6, and a seventh link L7, which are arranged in series from the base 11 side. The seventh link L7 is located at the tip of the robot arm 12. For example, the multiple joints J include a first joint J1, a second joint J2, a third joint J3, a fourth joint J4, a fifth joint J5, a sixth joint J6, and a seventh joint J7, which are arranged in series from the base 11 side. The position and orientation of the seventh link L7 have six degrees of freedom, including translational and rotational directions about three orthogonal axes. The robot arm 12 may be a so-called seven-axis robot having seven joints J. In other words, the robot arm 12 has redundancy. Redundancy is a characteristic in which the rotation angles of the multiple joints J corresponding to the position and orientation of the tip of the robot arm 12 are not uniquely determined.

[0026] The base 11 and the first link L1 are rotatably connected by a first joint J1. The first link L1 and the second link L2 are rotatably connected by a second joint J2. The second link L2 and the third link L3 are rotatably connected by a third joint J3. The third link L3 and the fourth link L4 are rotatably connected by a fourth joint J4. The fourth link L4 and the fifth link L5 are rotatably connected by a fifth joint J5. The fifth link L5 and the sixth link L6 are rotatably connected by a sixth joint J6. The sixth link L6 and the seventh link L7 are rotatably connected by a seventh joint J7.

[0027] A hand 14 may be connected to a seventh link L7 at the tip of the robot arm 12. In other words, the hand 14 is connected to the robot arm 12 so as to be rotatable around the rotation axis of the seventh joint J7. The hand 14 is an end effector attached to the robot arm 12.

[0028] In more detail, the multiple joints J may include a joint that functions as a shoulder joint. For example, the multiple joints J include a joint that has the functions of horizontal extension and horizontal flexion in the shoulder joint. The rotation axis of the joint that has the functions of horizontal extension and horizontal flexion in the shoulder joint extends in a substantially vertical direction. The multiple joints J may include a joint that has the functions of extension and flexion in the shoulder joint. The rotation axis of the joint that has the functions of extension and flexion in the shoulder joint extends in a substantially horizontal direction.

[0029] For example, the first joint J1 functions as a shoulder joint of the robot arm 12. The first joint J1 may have the functions of horizontal extension and horizontal flexion at the shoulder joint. The rotation axis of the first joint J1 extends in a substantially vertical direction.

[0030] For example, the second joint J2 functions as a shoulder joint of the robot arm 12. The second joint J2 may have the functions of extension and flexion at the shoulder joint. The rotation axis of the second joint J2 extends in a substantially horizontal direction.

[0031] For example, the third joint J3 functions as a shoulder joint of the robot arm 12. The third joint J3 may have the function of internal rotation and external rotation in a shoulder joint.

[0032] The multiple joints J may include a joint that functions as a wrist joint. For example, the multiple joints J include a joint that has the functions of internal rotation and external rotation or the functions of pronation and supination at the wrist joint. For example, the seventh joint J7 may have the functions of internal rotation and external rotation at the wrist joint. The sixth joint J6 may have the functions of pronation and supination at the wrist joint.

[0033] The multiple joints J may include an intermediate joint between a shoulder joint and a wrist joint. The intermediate joint may also be referred to as an elbow joint. The intermediate joint may have functions of extension and flexion at the intermediate joint, or functions of internal rotation and external rotation at the intermediate joint. The fourth joint J4 may have functions of extension and flexion at the intermediate joint. The fifth joint J5 may have functions of internal rotation and external rotation at the intermediate joint.

[0034] The robot arm 12 has a motor 12a (see FIG. 3) that rotates and drives each joint J. For example, the motor 12a is a servo motor. Each motor 12a has an encoder 12b (see FIG. 3).

[0035] The mobile body 100 may include a sensor 3 that detects objects (hereinafter simply referred to as "peripheral objects") around the mobile body 1. In this disclosure, "objects" includes both inanimate and animate objects. The sensor 3 is disposed on the mobile body 1. For example, the sensor 3 is disposed on a dolly 10 below the mobile body 1. In this example, the sensor 3 is a distance measurement sensor that measures the distance from the sensor 3 to the peripheral objects. For example, the sensor 3 is a LiDAR (Light Detection and Ranging) sensor. The sensor 3 has, for example, a light-emitting unit that emits laser light toward the periphery of the mobile body 1 and a light-receiving unit that receives the laser light reflected off the surface of the peripheral object. The sensor 3 measures the flight time of the laser light emitted from the light-emitting unit, hitting the surface of the peripheral object, and returning to the light-receiving unit. The sensor 3 measures the distance from the sensor 3 to the surface of the peripheral object based on the measured flight time. The sensor 3 may generate point cloud data based on the measured distance. The point cloud data is three-dimensional position information of the surface of the peripheral object. For example, the sensor 3 outputs the calculated point cloud data to the control device 6. The sensor 3 may repeatedly detect surrounding objects at a predetermined detection period while the mobile body 1 is moving. The sensor 3 may output the detection result of the sensor 3, i.e., the point cloud data, to the control device 6 every time the sensor 3 detects a surrounding object.

[0036] In this example, the mobile body 100 is equipped with multiple sensors 3. FIG. 2 is a schematic diagram showing the detection range of the sensor 3. FIG. 2 is a plan view of the mobile body 100, omitting the robot arm 12 and other components. The mobile body 100 may be equipped with a first sensor 3A, a second sensor 3B, and a third sensor 3C. The first sensor 3A, the second sensor 3B, and the third sensor 3C are disposed on the carriage 10. The first sensor 3A is disposed at the front of the carriage 10. For example, the first sensor 3A is disposed on the carriage 10, forward of the base 11 and approximately in the center in the left-right direction. The first sensor 3A detects objects in the three-dimensional space around the mobile body main body 1. The first sensor 3A may be a 3D LiDAR. The first sensor 3A scans the measurement light in the horizontal and vertical directions. In this example, the first sensor 3A scans the measurement light 360 degrees horizontally, as indicated by the two-dot chain line in FIG. 2. In the vertical direction, the first sensor 3A causes the measurement light to scan within a predetermined range including elevation and depression angles.

[0037] The second sensor 3B and the third sensor 3C may be disposed at the rear of the bogie 10. More specifically, the second sensor 3B and the third sensor 3C are disposed on the bogie 10 rearward of the base 11. The second sensor 3B is disposed at the left rear corner of the bogie 10, and the third sensor 3C is disposed at the right rear corner of the bogie 10. The second sensor 3B and the third sensor 3C may detect objects in a two-dimensional space in the horizontal direction around the mobile body 1. For example, the second sensor 3B and the third sensor 3C are 2D LiDARs. The second sensor 3B and the third sensor 3C scan the measurement light in the horizontal direction. The second sensor 3B and the third sensor 3C detect objects in a range in the horizontal direction that cannot be detected by the first sensor 3A. The second sensor 3B scans the measurement light at least to the left rear of the bogie 10. The third sensor 3C scans the measurement light at least to the right rear of the bogie 10. The scanning range of the measurement light by the second sensor 3B and the scanning range of the measurement light by the third sensor 3C partially overlap behind the carriage 10. In this example, the second sensor 3B scans the measurement light horizontally by approximately 270 degrees from the front to the right, including the left area of ​​the mobile body 1, as shown by the dashed line in FIG. 2. The third sensor 3C scans the measurement light horizontally by approximately 270 degrees from the front to the left, including the right area of ​​the mobile body 1, as shown by the dashed line in FIG. 2. The second sensor 3B and the third sensor 3C detect objects at approximately the same height. That is, the scanning plane of the measurement light by the second sensor 3B and the scanning plane of the measurement light by the third sensor 3C are at approximately the same height.

[0038] 2, since the base 11 is disposed behind the first sensor 3A, the first sensor 3A cannot properly scan the measurement light into the range F that overlaps with the base 11. On the other hand, since the second sensor 3B and the third sensor 3C are disposed behind the base 11, the second sensor 3B and the third sensor 3C can also scan the measurement light into the range F.

[0039] Hereinafter, when there is no need to distinguish between the first sensor 3A, the second sensor 3B, and the third sensor 3C, they will be simply referred to as "sensors 3."

[0040] FIG. 3 is a diagram showing the hardware configuration of the control device 6. The control device 6 controls the entire mobile body 1. The control device 6 causes the mobile body 1 to move autonomously while estimating the self-position of the mobile body 1. The control device 6 operates the motor 13a of the wheel 13 to move the mobile body 1. Furthermore, the control device 6 controls the motor 12a of the robot arm 12 to cause the robot arm 12 to perform a predetermined task. The control device 6 has a processor 61, a storage device 62, and a memory 63.

[0041] The processor 61 performs various types of arithmetic processing. For example, the processor 61 is formed of a processor such as a CPU (Central Processing Unit). The processor 61 may be formed of an MCU (Micro Controller Unit), an MPU (Micro Processor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), a system LSI, or the like. The processor 61 operates the motor 13a, causing the mobile body 1 to move autonomously.

[0042] The memory 62 stores programs and various data executed by the processor 61. For example, the memory 62 stores a control program. The memory 62 stores map information related to a map of the environment in which the mobile body 1 moves. For example, the map information includes a three-dimensional map and a two-dimensional map. The three-dimensional map is formed from three-dimensional point cloud data. For example, the three-dimensional map is a three-dimensional point cloud map. In the three-dimensional map, the three-dimensional shapes of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, are represented by point cloud data. The two-dimensional map is a planar map. For example, the two-dimensional map is a two-dimensional occupancy grid map. In the two-dimensional map, the planar shapes of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, are represented. For example, the two-dimensional map is formed by projecting the three-dimensional map onto a plane. The memory 62 is formed from a non-volatile memory, a hard disk drive (HDD), a solid state drive (SSD), or the like. The memory 63 temporarily stores data, etc. For example, the memory 63 is formed of a volatile memory.

[0043] 4 is a functional block diagram showing the configuration of the control system of the processor 61. The processor 61 realizes various functions by reading a control program from the storage device 62 into the memory 63 and expanding it. Specifically, the processor 61 functions as a state estimator 64 that estimates the state of the mobile body 1, a map generator 65 that generates a map of the environment in which the mobile body 1 moves, a path generator 66 that plans a path for the mobile body 1, a trajectory generator 67 that generates a target trajectory according to the path, and a movement controller 68 that moves the mobile body 1 according to the target trajectory. The processor 61 also functions as an operation amount calculator 69 that calculates the operation amount of the motor 13a.

[0044] The state estimator 64 performs self-position estimation. The state estimator 64 receives the detection results of the sensor 3, the detection results of the encoder 13b, and the map information in the memory 62. The map information is, for example, a three-dimensional map. The state estimator 64 compares the detection results of the sensor 3 with the map information to estimate the current position of the mobile body 1, i.e., its self-position. Here, the position of the mobile body 1 also includes the orientation of the mobile body 1, i.e., its attitude.

[0045] In this example, the state estimator 64 performs self-position estimation using the three-dimensional point cloud data of the first sensor 3 A. The state estimator 64 compares environmental information around the mobile body 1 obtained from the three-dimensional point cloud data of the first sensor 3 A with a three-dimensional map, and estimates the position of the mobile body 1 within the environment represented by the three-dimensional map, i.e., the self-position.

[0046] The map generator 65 generates a map based on the detection results of the sensor 3. Specifically, the map generator 65 generates or modifies a three-dimensional map based on the detection results of the sensor 3. In this example, before autonomous movement is performed, the three-dimensional map is generated using SLAM (Simultaneous Localization and Mapping) technology. Specifically, while the mobile body 1 is moving within the environment, the state estimator 64 and the map generator 65 acquire the detection results of the sensor 3 and perform self-position estimation and map generation in parallel. The generated map information, i.e., the three-dimensional map, is stored in the memory 62. When generating the map before autonomous movement is performed, the mobile body 1 is moved by manual operation by the user.

[0047] Furthermore, the map generator 65 updates the two-dimensional map. The two-dimensional map can also be updated during autonomous movement. The map generator 65 detects obstacles in the environment based on the detection results of the sensor 3 acquired while the mobile body 1 is moving, and updates the two-dimensional map.

[0048] The route generator 66 reads the destination and map information from the memory 62. The destination is set in advance in the memory 62. The map information at this time is, for example, a two-dimensional map. At this time, the route generator 66 may read intermediate points in addition to the destination. The state quantities (including the estimated position) of the mobile body 1 are input to the route generator 66 from the state estimator 64.

[0049] The path generator 66 generates a path from the current position of the mobile body 1 to the destination based on map information. The path generator 66 references the map information to generate a path that avoids interference with obstacles, etc. If a passage is set in the environment, the path generator 66 generates a path along the passage. For example, the path generator 66 generates a path using an A-star search algorithm, an RRT algorithm, a Dijkstra algorithm, or a geometric approach. The path generator 66 outputs an array of positions through which the mobile body 1 passes as a path to the trajectory generator 67. Each position includes the attitude of the mobile body 1 in addition to position information.

[0050] The trajectory generator 67 generates a target trajectory from the current position of the mobile body 1 according to the generated path. The trajectory generator 67 generates the target trajectory of the mobile body 1 using a predetermined method (for example, line-of-sight guidance law). The state quantities of the mobile body 1 are input to the trajectory generator 67 from the state estimator 64. The trajectory generator 67 calculates a command speed for the mobile body 1.

[0051] Alternatively, the trajectory generator 67 may calculate the command speed by model predictive control (MPC). Model predictive control determines a control input, i.e., a speed command, by sequentially solving an optimization problem based on a model of the mobile body 1. The trajectory generator 67 predicts future state quantities from the current state quantities of the mobile body 1 and obstacles, calculates an optimal path for the mobile body 1, and calculates a moving speed from the current position to the target position to follow that path as a command speed.

[0052] The command speed calculated by the trajectory generator 67 is input to the movement controller 68. The movement controller 68 outputs a command value according to the command speed to the operation amount calculator 69.

[0053] The movement controller 68 executes control to avoid interference between the mobile body 1 and an obstacle. The movement controller 68 monitors the approach of the mobile body 1 to an obstacle based on the detection results of the sensors 3. In this example, the movement controller 68 monitors the approach of the mobile body 1 to an obstacle using all of the detection results of the first sensor 3A, the second sensor 3B, and the third sensor 3C. For example, the movement controller 68 slows down or stops the mobile body 1 depending on the distance between the mobile body 1 and the obstacle.

[0054] The manipulated variable calculator 69 distributes the command value to the plurality of motors 13a and calculates the command manipulated variable for each of the plurality of motors 13a. For example, the manipulated variable is the rotation speed or torque of the motor.

[0055] Each motor 13a operates according to a command operation amount. In some cases, the motor 13a is provided with a dedicated controller for operating the motor 13a. For example, if the motor 13a is a servo motor, the motor 13a further includes a servo amplifier. In this case, the servo amplifier operates the motor 13a according to the command operation amount. As a result, the mobile body 1 moves.

[0056] Next, a description will be given of the basic operation of the moving body 100. Fig. 5 is a flowchart of the basic operation of the moving body 100. The moving body 100 repeatedly executes the following processing at a predetermined control cycle.

[0057] First, in step S1, the state estimator 64 acquires surrounding environment information. Specifically, the state estimator 64 acquires the detection signal of the sensor 3 and the detection signal of the encoder 13b.

[0058] Next, in step S2, the state estimator 64 performs self-localization.

[0059] Subsequently, in step S3, the route generator 66 executes route planning, generating a route for the mobile body 1 based on the map information, the estimated position of the mobile body 1, and the destination.

[0060] In step S4, the trajectory generator 67 calculates a command velocity from the estimated position of the mobile body 1 so as to follow the generated path.

[0061] In step S5, the movement controller 68 causes the moving body 1 to move in accordance with the command speed.

[0062] By repeating the above processing, the moving body 100 estimates its own position and moves autonomously to the destination.

[0063] Next, a more detailed description will be given of the self-location estimation by the state estimator 64. Fig. 6 is a detailed functional block diagram of the state estimator 64. The state estimator 64 has an acquirer 641, a classifier 642, and a comparator 643 as functional blocks.

[0064] The acquirer 641 acquires three-dimensional point cloud data indicating the position information of structures around the mobile body 1 from the detection signals of the sensor 3. The structures are immovable architectural structures in a room, such as walls, pillars, ceilings, or floors. The structures exclude movable obstacles.

[0065] Specifically, in acquiring the three-dimensional point cloud data of the structure, the acquirer 641 selects and acquires the three-dimensional point cloud data of the structure from three-dimensional point cloud data indicating three-dimensional position information of objects around the mobile body 1. In more detail, the acquirer 641 acquires the three-dimensional point cloud data of all objects around the mobile body 1 from the detection signal of the sensor 3, and selects the three-dimensional point cloud data of the structure by excluding three-dimensional point cloud data of obstacles and the like from the three-dimensional point cloud data of all objects.

[0066] The classifier 642 extracts at least one point from each of the multiple points included in the three-dimensional point cloud data of the structure, and classifies the extracted points (hereinafter also referred to as extracted points) into corresponding regions in three directions in the coordinate system of the mobile body 1 until the number of points included in each region reaches a specified number. The three directions of the mobile body 1 include a first region in the front-to-back direction of the mobile body 1, a second region in the left-to-right direction of the mobile body 1, and a third region in the up-to-down direction of the mobile body 1. In this example, the classifier 642 extracts one point from each of the multiple points included in the three-dimensional point cloud data of the structure, and classifies the extracted points into corresponding regions on the mobile body 1 one by one. Note that the classifier 642 may also extract multiple points from each of the multiple points included in the three-dimensional point cloud data of the structure, and classify the extracted points into corresponding regions one by one.

[0067] Specifically, the classifier 642 randomly extracts one point from multiple points included in the 3D point cloud data of the structure. Then, the classifier 642 searches for a region to which the extracted point corresponds and classifies the extracted point into the corresponding region. The classifier 642 randomly extracts and classifies points one by one until the number of points included in each region reaches a specified number. The specified number is, for example, 67, a value determined by parameter adjustment.

[0068] Here, a method for classifying extracted points into each region by the classifier 642 will be described. First, the classifier 642 finds the height of the extracted point. If the height of the extracted point is a predetermined value, the classifier 642 searches for a third region as a corresponding region of the extracted point, and classifies the extracted point into the third region. The third area is filled with points until the number of points reaches a certain number. The predetermined values ​​include a first predetermined value and a second predetermined value. The first predetermined value is the height of the floor and includes an error of, for example, ±10 cm. The second predetermined value is the height of the ceiling and includes an error of, for example, ±10 cm. A floor or a ceiling is an example of a structure. In this way, the classifier 642 classifies points included in the three-dimensional point cloud data of the floor or ceiling. The third region is searched as the corresponding region of the extracted points from and the extracted points are searched until the number of points included in the third region reaches a predetermined number. It falls into the third category.”

[0069] If the height of the extracted point is not a predetermined value, the classifier 642 searches the first region or the second region as the corresponding region of the extracted point and classifies the extracted point into the first region or the second region. The number of points included in the first or second area is determined7 is a schematic plan view showing the relationship between the first region Z1 and the second region Z2 of the mobile body 1 and point P included in the three-dimensional point cloud data of the wall W, and simplifies the three-dimensional representation into two dimensions. In FIG. 7, the center of gravity of the mobile body 1 is the origin, and the front-to-back direction of the mobile body 1 is defined as the X-axis, and the left-to-right direction of the mobile body 1 is defined as the Y-axis. The forward direction of the mobile body 1 is defined as the positive direction of the X-axis, and the leftward direction of the mobile body 1 is defined as the positive direction of the Y-axis. The first region Z1 and the second region Z2 are indicated by hatching.

[0070] The first region Z1 is a region where the absolute value of the X coordinate is greater than the absolute value of the Y coordinate. The region of the first region Z1 where the X coordinate is positive is the front region Z11. The region of the first region Z1 where the X coordinate is negative is the back region Z12.

[0071] The second region Z2 is a region where the absolute value of the Y coordinate is greater than the absolute value of the X coordinate. The region of the second region Z2 where the Y coordinate is positive is the left region Z21. The region of the second region Z2 where the Y coordinate is negative is the right region Z22.

[0072] FIG. 7 shows three-dimensional point cloud data of a wall W acquired by an acquirer 641. The three-dimensional point cloud data of the wall W includes a plurality of points P. The wall W is an example of a structure. The classifier 642 obtains the X coordinate and the Y coordinate of a point P extracted from the plurality of points P. If the absolute value of the X coordinate of the extracted point P is greater than the absolute value of the Y coordinate, the classifier 642 searches a first region Z1 as a corresponding region of the extracted point P, and classifies the extracted point P into the first region Z1. In the first area Z1, the number of points included reaches a specified number. On the other hand, if the absolute value of the Y coordinate of the extracted point P is greater than the absolute value of the X coordinate, the classifier 642 searches the second region Z2 as the corresponding region of the extracted point P, and classifies the extracted point P into the second region Z2. The second area Z2 is included until the number of points reaches a specified number. Classify.

[0073] In this way, the classifier 642 classifies the points P included in the three-dimensional point cloud data of the wall W located in the front-rear direction of the moving body 1. The first area Z1 is searched as a corresponding area of ​​the extracted points extracted from, and the extracted points are searched until the number of points included in the first area Z1 reaches a predetermined number. The classifier 642 classifies the points P included in the three-dimensional point cloud data of the wall W located in the left and right directions of the main body 1 of the moving object into the first region Z1. The second area Z2 is searched as a corresponding area of ​​the extracted points extracted from the second area Z2, and the extracted points are searched until the number of points included in the second area Z2 reaches a predetermined number. Classify it into the second area Z2.

[0074] In FIG. 7, the classifier 642 obtains the X and Y coordinates of the extracted point P and classifies the extracted point P into the first region Z1 or the second region Z2. However, other methods may be used to classify the extracted point P into the first region Z1 or the second region Z2. FIG. 8 is a schematic diagram illustrating another method for classifying the extracted point P into the first region Z1 or the second region Z2 of the mobile body 1. FIG. 8 corresponds to FIG. 7. FIG. 8 is a schematic plan view showing the relationship between the first region Z1 and the second region Z2 of the mobile body 1 and the point P included in the three-dimensional point cloud data of the wall W, where the three-dimensional representation is simplified to two dimensions. In FIG. 8, the same reference numerals as in FIG. 7 have the same configuration, and therefore their description will be omitted.

[0075] The classifier 642 determines the angle between the normal direction N of the extracted point P and the X axis. If the angle is smaller than 45°, the classifier 642 classifies the extracted point P into the first region Z1. On the other hand, if the angle is larger than 45°, the classifier 642 classifies the extracted point P into the second region Z2.

[0076] In this way, the classifier 642 classifies the points P included in the three-dimensional point cloud data of the walls W located in the front-to-back direction of the mobile body 1 into the first region Z1. The classifier 642 also classifies the points P included in the three-dimensional point cloud data of the walls W located in the left-to-right direction of the mobile body 1 into the second region Z2.

[0077] In classifying extracted points into each region, the classifier 642 does not classify the extracted points into each region more than a predetermined number of times. Specifically, when classifying an extracted predetermined point into a predetermined region corresponding to the predetermined point, if the classifier 642 determines that the number of points included in the predetermined region has reached a predetermined number, the classifier 642 does not classify the predetermined point into the predetermined region.

[0078] The classifier 642 ends the classification of the extracted points into each region when it determines that the total number of points classified into each region has reached a predetermined number. Specifically, the classifier 642 ends the classification of the extracted points into each region when it determines that the total number of points classified into the first region, the second region, and the third region has reached a predetermined number. The predetermined number is the total number of the specified numbers of each region. The specified number is, for example, 201 (specifically, the specified number 67 × 3 = 201), and is a value determined by parameter adjustment. In other words, the classifier 642 ends the classification of the extracted points into each region when it determines that the number of points classified into each region has reached a predetermined number. Specifically, the classifier 642 ends the classification of the extracted points into each region when it determines that the number of points classified into the first region, the second region, and the third region have each reached a predetermined number.

[0079] When classifying extracted points into each region, if the classifier 642 determines that the total number (cumulative number) of points extracted up to that point is equal to or greater than a threshold, it classifies subsequently extracted points into the corresponding region even if the number of points included in the region corresponding to the point reaches a predetermined number. The threshold is, for example, 10 times a predetermined number (specifically, the predetermined number 201 × 10 = 2010), and is a value determined by parameter adjustment.

[0080] Specifically, if the areas corresponding to the extracted points are concentrated in one area and the total number of points classified into each area does not reach a predetermined number, the classification of the extracted points into each area will not end. Therefore, if a number of points equal to or greater than the threshold is extracted but the total number of points classified into each area does not reach a predetermined number, points extracted thereafter will be classified into the corresponding area even if the number of points included in the area corresponding to that point reaches the predetermined number. As a result, the classification of the extracted points into each area can be completed when the total number of points classified into each area reaches the predetermined number.

[0081] The comparator 643 compares the points classified into each region with points of three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body 1 moves, to estimate the self-position of the mobile body 1. Specifically, the comparator 643 compares all points classified into each region with points included in the three-dimensional point cloud data of the three-dimensional map (map information), to estimate the self-position of the mobile body 1.

[0082] The comparator 643 estimates the self-location of the mobile body 1 using, for example, Monte Carlo Localization (MCL) or Adaptive Monte Carlo Localization (AMCL). Alternatively, the comparator 643 estimates the self-location of the mobile body 1 using, for example, Iterative Closest Point (ICP) scan matching or Normal Distributions Transform (NDT) scan matching.

[0083] Next, the self-position estimation process (an example of a method for controlling the moving body 100) by the state estimator 64 will be described with reference to a flowchart. FIG.

[0084] In step S11, the acquirer 641 performs an acquisition process. Specifically, the acquirer 641 acquires three-dimensional point cloud data indicating the position information of structures around the main body 1 of the moving object.

[0085] In step S12, the classifier 642 performs classification processing. Specifically, the classifier 642 extracts at least one point from each of the multiple points included in the three-dimensional point cloud data of the structure, and classifies the extracted points into corresponding areas among areas in three directions in the coordinate system of the mobile body 1 until the number of points included in each area reaches a specified number.

[0086] In step S13, the comparator 643 performs a comparison process. Specifically, the comparator 643 compares the points classified into each region with the points of the three-dimensional point cloud data included in the map information relating to the map of the environment in which the mobile body 1 moves, and estimates the self-position of the mobile body 1.

[0087] 10 is a flowchart of a subroutine of the acquisition process (step S11). First, in step S21, the acquirer 641 acquires three-dimensional point cloud data of all objects around the mobile body 1 (hereinafter also referred to as all point cloud data) from the detection signals of the sensor 3.

[0088] In step S22, the acquirer 641 removes point cloud data of the moving body 100 at a long distance from the entire point cloud data. Specifically, the acquirer 641 searches for a first point whose distance from the moving body 100 is greater than a first set value from among all points included in the entire point cloud data, and removes the first point from all points. The first set value is, for example, a numerical value determined by parameter adjustment.

[0089] In step S23, the acquirer 641 removes point cloud data in the vicinity of the moving body 100 from all point cloud data. Specifically, the acquirer 641 searches for a second point whose distance from the moving body 100 is smaller than a second set value from all points included in the all point cloud data, and removes the second point from all points. The second set value is smaller than the first set value, and is a numerical value determined by parameter adjustment, for example.

[0090] In step S24, the acquirer 641 removes the point cloud data of the moving body 1 from all the point cloud data. Specifically, the acquirer 641 searches for a third point that measures the moving body 1 itself from all the points included in the all the point cloud data, and removes the third point from all the points.

[0091] In step S25, the acquirer 641 removes point cloud data of a predetermined height from all point cloud data. Specifically, the acquirer 641 searches for a fourth point whose height is outside a predetermined range from all points included in all point cloud data, and removes the fourth point from all points.

[0092] The predetermined ranges are, for example, a first predetermined value, a second predetermined value, and a third predetermined range. The first predetermined value is the floor height and includes an error of, for example, ±10 cm. Points whose height is the first predetermined value include points included in the three-dimensional point cloud data of the floor. The second predetermined value is the ceiling height and includes an error of, for example, ±10 cm. Points whose height is the second predetermined value include points included in the three-dimensional point cloud data of the ceiling. The third predetermined range is the range from the height of the obstacle to the height of the ceiling. Points whose height is within the third predetermined range include points included in the three-dimensional point cloud data of the wall.

[0093] In step S26, the acquirer 641 acquires three-dimensional point cloud data of the structure. Specifically, the acquirer 641 acquires three-dimensional point cloud data of the wall, floor, or ceiling. Then, the process returns to the flowchart in FIG.

[0094] 11 is a flowchart of a subroutine of the classification process (step S12). First, in step S31, the classifier 642 randomly extracts one point from a plurality of points included in the three-dimensional point cloud data of the structure acquired by the acquirer 641.

[0095] In step S32, the classifier 642 searches for a region (hereinafter also referred to as a corresponding region) to which the extracted point corresponds among the regions in three directions in the coordinate system of the mobile body 1. Specifically, as explained in the method of classifying the extracted points into each region by the classifier 642, the classifier 642 searches for the first region, the second region, or the third region as the corresponding region of the extracted point.

[0096] In step S33, the classifier 642 determines whether the number of points included in the corresponding region reaches a specified number.

[0097] If the classifier 642 determines that the number of points included in the corresponding region does not reach the specified number, then in step S36, the classifier 642 classifies the extracted points into the corresponding region. If the classifier 642 determines that the number of points included in the corresponding region reaches the specified number, then in step S34, the classifier 642 determines whether the total number of extracted points extracted up to that point, excluding the currently extracted point, is equal to or greater than a threshold.

[0098] If the classifier 642 determines that the total number of extracted points up to the previous point is equal to or greater than the threshold, the classifier 642 classifies the extracted point into a corresponding region in step S36. If the classifier 642 determines that the total number of extracted points up to the previous point is not equal to or greater than the threshold, the classifier 642 removes the extracted point without classifying it into a corresponding region in step S35.

[0099] In step S37, the classifier 642 determines whether the total number of points classified into each region has reached a predetermined number.

[0100] If the classifier 642 determines that the total number of points classified into each region has reached a predetermined number, then in step S38, the classifier 642 ends the classification of the extracted points into each region, and then returns to the flowchart of FIG.

[0101] If the classifier 642 determines that the total number of points classified into each region has not reached a predetermined number, in step S31, the classifier 642 randomly extracts one point from the multiple points included in the 3D point cloud data of the structure. At this time, the classifier 642 removes the most recently extracted point from the multiple points and extracts a new point. Then, the classifier 642 repeats steps S32 to S37.

[0102] FIG. 12 is a schematic diagram illustrating the classification process performed by the classifier 642. FIG. 12 corresponds to FIG. 7. FIG. 12 is a planar schematic diagram illustrating the relationship between the first region Z1 and the second region Z2 of the mobile body 1 and points P included in the three-dimensional point cloud data of the wall W, where the three-dimensional representation is simplified to two dimensions. In FIG. 12, the same reference numerals as in FIG. 7 have the same configuration, and therefore their description will be omitted. In FIG. 12, an obstacle 30 is present in the left-right direction (Y direction) of the mobile body 1, and the three-dimensional point cloud data of the wall W in the left-right direction of the mobile body 1 acquired by the acquirer 641 is extremely small. In other words, the number of points P included in the three-dimensional point cloud data of the wall W in the left-right direction of the mobile body 1 acquired by the acquirer 641 is extremely small compared to the number of points P included in the three-dimensional point cloud data of the wall W in the front-rear direction (X direction) of the mobile body 1 acquired by the acquirer 641.

[0103] The classifier 642 classifies points P included in the three-dimensional point cloud data of the wall W located in the front-to-rear direction of the mobile body 1 into a first region Z1, and classifies points P included in the three-dimensional point cloud data of the wall W located in the left-to-right direction of the mobile body 1 into a second region Z2. Specifically, the classifier 642 classifies a specified number of points P into the first region Z1, and a specified number of points P into the second region Z2. In FIG. 12, the points P classified by the classifier 642 are shown surrounded by dashed circles, and the specified number of each region is set to three for ease of understanding. In this way, the classifier 642 classifies the same number of points P into the first region Z1 and the second region Z2, even if the number of points P in the Y direction acquired by the acquirer 641 is significantly smaller than the number of points P in the X direction acquired by the acquirer 641.

[0104] According to the above-mentioned mobile body 100, the control device 6 (specifically, the state estimator 64) acquires three-dimensional point cloud data of structures around the mobile body main body 1, extracts at least one point from each of the multiple points included in the three-dimensional point cloud data of the structure, classifies the extracted points into corresponding areas among the areas in three directions of the mobile body main body 1 until the number of points included in each area reaches a specified number, and compares the points classified into each area with the points of the three-dimensional point cloud data included in the map information to estimate the self-position of the mobile body main body 1.

[0105] As a result, even if the amount of point cloud data for structures in a specific direction of the mobile body 1 becomes extremely small due to the position of an obstacle relative to the mobile body 100, the control device 6 classifies points into each area of ​​the mobile body 1 in three directions until the number of points included in each of the areas in the three directions of the mobile body 1 reaches a specified number, and then compares the points classified into each area with the points of the three-dimensional point cloud data included in the map information. Therefore, the balance between the areas in the three directions of the mobile body 1 is improved in the scan matching between the point cloud data classified into each area and the three-dimensional point cloud data included in the map information, and a simple comparison operation reduces the decrease in accuracy of self-location estimation of the mobile body 1 without increasing the calculation load.

[0106] In addition, the control device 6 extracts points one by one from the multiple points included in the three-dimensional point cloud data of the structure, and classifies the extracted points one by one into corresponding areas among the three directional areas until the number of points included in each area reaches a specified number, thereby reducing the calculation load compared to when all points included in the three-dimensional point cloud data of the structure are classified into each area.

[0107] Furthermore, the control device 6 selects and acquires the three-dimensional point cloud data of the structure from the three-dimensional point cloud data of the objects around the mobile body 1, so that the three-dimensional point cloud data of the structure can be easily acquired.

[0108] Furthermore, the control device 6 does not classify the extracted points into each area beyond a specified number, thereby reducing the imbalance in the number of points classified into each area. This improves the balance between the areas in three directions of the mobile body 1 in scan matching between the point cloud data classified into each area and the three-dimensional point cloud data included in the map information, thereby reducing the decrease in accuracy of self-position estimation of the mobile body 1.

[0109] Furthermore, when the control device 6 determines that the total number of points classified into each area has reached a predetermined number, it ends the classification of the extracted points into each area, thereby reducing the imbalance in the number of points classified into each area. This improves the balance between the areas in three directions of the mobile body 1 in scan matching between the point cloud data classified into each area and the three-dimensional point cloud data included in the map information, thereby reducing the decrease in accuracy of self-location estimation of the mobile body 1.

[0110] Furthermore, since the predetermined number is the total number of the prescribed numbers for each region, when the control device 6 determines that the number of points classified into each region has reached the prescribed number, it ends the classification of the extracted points into each region.

[0111] Furthermore, when the control device 6 determines that the total number of points extracted up to that point is equal to or greater than the threshold, it classifies subsequently extracted points into the corresponding area even if the number of points included in the area reaches a predetermined number, so that the classification of extracted points into each area can be completed when the total number of points classified into each area reaches a predetermined number, thereby preventing an increase in the calculation load.

[0112] Furthermore, since the sensor 3 is disposed at the bottom of the mobile body 1, the sensor 3 may not be able to detect the structure due to an obstacle near the mobile body 100. However, since the mobile body 100 is equipped with the control device 6 described above, even if the amount of point cloud data of the structure in a specific direction of the mobile body 1 acquired by the acquirer 641 becomes extremely small depending on the position of the obstacle relative to the mobile body 100, the decrease in accuracy of self-position estimation of the mobile body 1 is mitigated. Therefore, the mobile body 100 of the present disclosure is suitable when the sensor 3 is disposed at the bottom of the mobile body 1.

[0113] The mobile body 1 is a mobile robot that includes a robot arm 12. When the mobile body 1 moves within a facility such as a store, hospital, or nursing home, even if the amount of point cloud data of structures in a specific direction of the mobile body 1 acquired by the acquirer 641 within the facility becomes extremely small, the degradation of accuracy in estimating the self-position of the mobile body 1 is mitigated.

[0114] The control device 6 may control the robot arm 12 when performing autonomous movement. Fig. 13 is a functional block diagram showing the configuration of a control system of the processor 61 according to a modified example. The processor 61 may function as an arm controller 611 that controls the robot arm 12.

[0115] The arm controller 611 operates the robot arm 12. For example, the arm controller 611 transforms the robot arm 12 into a target shape. The arm controller 611 may maintain the robot arm 12 in the target shape. The arm controller 611 may operate the robot arm 12 by continuously changing the shape of the robot arm 12.

[0116] The arm controller 611 generates command values ​​according to a target shape of the robot arm 12. Based on the command values, the arm controller 611 calculates command operation amounts for each of the multiple motors 12a. For example, the operation amounts are the rotational speed or torque of the motors.

[0117] The arm controller 611 may maintain the robot arm 12 in a fixed shape when the moving body 100 is moving, and may operate the robot arm 12 when the robot arm 12 is performing work.

[0118] For example, when the moving body 100 is traveling, the arm controller 611 maintains the robot arm 12 in a traveling shape. In other words, when the moving body 100 is traveling, the arm controller 611 fixes the shape of the robot arm 12 and prohibits the robot arm 12 from moving.

[0119] Fig. 14 is a side view of the mobile body 1 when the robot arm 12 is in the running shape. Fig. 15 is a plan view of the mobile body 1 when the robot arm 12 is in the running shape.

[0120] For example, the robot arm 12 in the running configuration is positioned at a relatively high position. For example, the robot arm 12 in the running configuration is bent at an intermediate joint between the shoulder joint and the wrist joint, for example, at the fourth joint J4, with the portion between the base 11 and the intermediate joint extending diagonally downward and rearward from the base 11, and the portion between the intermediate joint and the wrist joint extending forward from the intermediate joint. That is, the robot arm 12 in the running configuration has the intermediate joint pulled rearward and bent at the intermediate joint. As a result, the portion of the robot arm 12 closer to the hand than the intermediate joint is positioned at a relatively high position. Furthermore, the hand of the robot arm 12 is positioned relatively rearward.

[0121] The robot arm 12 in the traveling configuration is positioned higher than the first sensor 3A of the mobile body 1. The detection range of the first sensor 3A extends three-dimensionally from the first sensor 3A. The space above the first sensor 3A is included in the detection range of the first sensor 3A. Because the robot arm 12 is positioned above the first sensor 3A, it may block part of the detection range of the first sensor 3A. The detection results of the first sensor 3A that correspond to the robot arm 12 are treated as invalid. The higher the position of the robot arm 12, the farther the robot arm 12 is from the first sensor 3A. The farther the robot arm 12 is from the first sensor 3A, the smaller the area of ​​the detection range of the first sensor 3A that is blocked by the robot arm 12 tends to be. Therefore, in the traveling configuration, the detection range of the first sensor 3A is relatively large.

[0122] Furthermore, the robot arm 12 in the traveling configuration has a relatively small forward protrusion amount from the base 11. As the forward protrusion amount of the robot arm 12 decreases, the detection range of the first sensor 3A is expanded diagonally upward and forward from the first sensor 3A.

[0123] The width direction size of the overall shape of the running-shaped robot arm 12 in a plan view is relatively small. For example, in the running shape, the second link L2 is located at the outermost position in the width direction. Of the multiple links L, the links other than the second link L2 are located more inward in the width direction than the second link L2. By making the width direction size of the overall shape of the running-shaped robot arm 12 in a plan view relatively small, the possibility of interference between the robot arm 12 and other objects located in the width direction during running can be reduced. Note that the running-shaped robot arm 12 may be configured such that the first link L1 and the second link L2 are located forward of the rotation axis of the first joint J1 by rotating the first link L1 forward about the rotation axis of the first joint J1. This further reduces the width direction size of the second links L2 of the two robot arms, i.e., the width direction size of the overall shape of the robot arm 12 in a plan view.

[0124] The overall shape of the robot arm 12 in the traveling configuration in a plan view is contained within the carriage 10 in the front-to-rear direction. This reduces the possibility of interference between the robot arm 12 and other objects located in the front-to-rear direction when traveling.

[0125] In addition, the shapes of the two robot arms 12 in terms of their running shapes do not have to be completely identical. That is, the shapes of the two robot arms 12 may be slightly different. For example, the height of the tip of one robot arm 12 may be different from the height of the tip of the other robot arm 12. The rotation angle of the seventh joint J7 of one robot arm 12 may be different from the rotation angle of the seventh joint J7 of the other robot arm 12.

[0126] For example, when the robot arm 12 is performing work, the arm controller 611 operates the robot arm 12. In other words, when the robot arm 12 is performing work, the arm controller 611 permits the operation of the robot arm 12 and allows the robot arm 12 to freely operate. For example, after the mobile body 1 has reached its destination, the arm controller 611 operates the robot arm 12 to perform work by the robot arm 12.

[0127] Other Embodiments As described above, the above embodiment has been described as an example of the technology disclosed in this application. However, the technology of the present disclosure is not limited to this and can be applied to embodiments in which modifications, substitutions, additions, omissions, etc. are made as appropriate. Furthermore, the components described in the above embodiment can be combined to create new embodiments. Furthermore, the components described in the accompanying drawings and detailed description may include not only components essential for solving the problem, but also components that are not essential for solving the problem in order to exemplify the technology. Therefore, the fact that these non-essential components are described in the accompanying drawings or detailed description should not be interpreted as immediately determining that these non-essential components are essential.

[0128] For example, the mobile body 1 is not limited to a robot, but may be a mobile device such as a drone, a ship, or a vehicle. The location where the mobile body 1 moves is not limited to a passageway, but may be a road or a waterway. The mobile body 1 does not need to include the robot arm 12. The base 11 may be rotatable relative to the cart 10. The wheels 13 are not limited to omnidirectional wheels. If the wheels 13 are omnidirectional wheels, they may be omniwheels.

[0129] The sensor 3 is not limited to LiDAR. The sensor 3 may be a two-dimensional or three-dimensional camera. The sensor 3 may be a three-dimensional scanner. The sensor 3 may be disposed in a portion of the mobile body 1 other than the carriage 10. The number of sensors 3 is not limited to three. The number of sensors 3 may be one, two, four, or more. The scanning ranges of the measurement light of the first sensor 3A, the second sensor 3B, and the third sensor 3C described above are merely examples. For example, the first sensor 3A may scan the measurement light over an area including at least the area in front of the mobile body 1. The first sensor 3A may scan the measurement light over a range from the left rear to the right rear of the mobile body 1, including the area in front of the mobile body 1. Note that each of the second sensor 3B and the third sensor 3C may scan the measurement light 360 degrees horizontally. The second sensor 3B may scan the measurement light over a predetermined range (not limited to 270 degrees) including the area in the left rear of the mobile body 1. For example, the second sensor 3B may cause the measurement light to scan 360 degrees horizontally. The third sensor 3C may cause the measurement light to scan a predetermined range (not limited to 270 degrees) including the area to the right rear of the mobile body 1. The third sensor 3C may cause the measurement light to scan 360 degrees horizontally.

[0130] In the embodiment, the classifier 642 classifies the points included in the three-dimensional point cloud data of the floor or ceiling acquired by the acquirer 641 into a third region of the mobile body 1, but the points included in the three-dimensional point cloud data of the floor and ceiling may also be classified into the third region.

[0131] In an embodiment, when classifying extracted points into each region, if the classifier 642 determines that the total number of points extracted up to that point is equal to or greater than a threshold, it classifies subsequently extracted points into the corresponding region even if the number of points contained in the region corresponding to that point reaches a specified number; however, it is not necessary to classify subsequently extracted points into the corresponding region if the number of points contained in the region corresponding to that point reaches a specified number.

[0132] The flowcharts are merely examples. Steps in the flowcharts may be changed, replaced, added, omitted, etc. as appropriate. The order of steps in the flowcharts may also be changed, and serial processing may be performed in parallel.

[0133] The control method can be realized by other devices without being limited to the control device 6 described above. The control program can be realized by other devices without being limited to the control device 6 described above.

[0134] The functionality of the elements disclosed herein may be implemented using one or more circuits or processing circuits, including general-purpose processors, special-purpose processors, integrated circuits, ASICs (Application Specific Integrated Circuits), FPGAs (Field Programmable Gate Arrays), and / or conventional circuitry. The functionality of the elements disclosed herein may be implemented using one or more circuits or processing circuits, including combinations of general-purpose processors, special-purpose processors, integrated circuits, ASICs, FPGAs, and conventional circuitry. The one or more circuits or processing circuits may be programmed using one or more programs stored together or separately in one or more memories or otherwise configured to perform the disclosed functions. A processor is considered a processing circuit or circuitry because it includes transistors and other circuits. A processor may also be a programmed processor that executes a program stored in a memory. In this disclosure, a circuit, unit, or means is hardware that performs the recited functions alone or in combination with each other, or hardware that is programmed to perform the recited functions alone or in combination with each other. The hardware may be any hardware disclosed herein that is programmed or configured to perform the recited functions.

[0135] A computer program containing computer instructions is stored in memory. The computer instructions provide logic and routines that enable hardware to perform the methods disclosed herein. The hardware includes, for example, processing circuits or circuitry. The computer program may be implemented in a known format in a computer-readable storage medium, a computer program product, a memory device, a recording medium such as a CD-ROM or DVD, and / or the memory of FPGAs or ASICs.

[0136] [Aspect] The above-described embodiment is a specific example of the following aspects.

[0137] (Aspect 1) The mobile body 100 comprises a mobile body 1, a sensor 3 that detects objects around the mobile body 1, and a control device 6 that causes the mobile body 1 to move autonomously while estimating the self-position of the mobile body 1 based on the detection signal of the sensor 3. The control device 6 acquires three-dimensional point cloud data indicating the position information of structures around the mobile body 1 from the detection signal of the sensor 3, extracts at least one point from each of the multiple points included in the three-dimensional point cloud data of the structure, classifies the extracted points into corresponding areas among the three-directional areas in the coordinate system of the mobile body 1 until the number of points included in each area reaches a specified number, and compares the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body 1 moves to estimate the self-position of the mobile body 1.

[0138] According to this configuration, even if the point cloud data of structures in a particular direction of the mobile body 100 becomes extremely small due to the position of the obstacle relative to the mobile body 100, a simple comparison operation reduces the decrease in accuracy of self-position estimation of the mobile body 1 without increasing the calculation load.

[0139] (Aspect 2) In the moving object 100 according to aspect 1, When acquiring the three-dimensional point cloud data of the structure, the control device 6 selects and acquires the three-dimensional point cloud data of the structure from the three-dimensional point cloud data that indicates the three-dimensional position information of objects around the mobile body 1.

[0140] According to this configuration, three-dimensional point cloud data of a structure can be easily acquired.

[0141] (Aspect 3) In the moving object 100 according to the first or second aspect, In classifying the extracted points into each region, the control device 6 does not classify the extracted points into each region more than a specified number of times.

[0142] This configuration can reduce the bias in the number of points classified into each region.

[0143] (Aspect 4) In the moving body 100 according to any one of aspects 1 to 3, When the control device 6 determines that the total number of points classified into each area has reached a predetermined number in classifying the extracted points into each area, it terminates the classification of the extracted points into each area.

[0144] This configuration can reduce the bias in the number of points classified into each region.

[0145] (Aspect 5) In the moving body 100 according to any one of aspects 1 to 4, The predetermined number is the total number of the prescribed numbers of each area.

[0146] According to this configuration, the predetermined number is the total number of the prescribed numbers for each region, so when the number of points classified into each region reaches the prescribed number, the classification of extracted points into each region can be completed.

[0147] (Aspect 6) In the moving body 100 according to any one of aspects 1 to 5, When classifying extracted points into each region, if the control device 6 determines that the total number of points extracted up to that point is equal to or greater than a threshold value, it classifies subsequently extracted points into the corresponding region even if the number of points contained in the region corresponding to that point reaches a specified number.

[0148] This configuration can prevent an increase in the calculation load.

[0149] (Aspect 7) In the moving body 100 according to any one of aspects 1 to 6, The sensor 3 is disposed at the bottom of the moving body 1 .

[0150] According to this configuration, the moving body 100 of the present disclosure is suitable for the case where the sensor 3 is disposed at the bottom of the moving body main body 1.

[0151] (Aspect 8) In the moving body 100 according to any one of aspects 1 to 7, The mobile body 1 includes a robot arm 12 and is a mobile robot.

[0152] According to this configuration, when the mobile body 1 moves within a facility such as a store, hospital, or nursing home, even if there is extremely little point cloud data of structures in a particular direction of the mobile body 1 within the facility, the decrease in accuracy of self-position estimation of the mobile body 1 is mitigated.

[0153] (Aspect 9) The control device 6 of the mobile body is a control device 6 of the mobile body that causes the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1, and is equipped with an acquirer 641 that acquires three-dimensional point cloud data indicating the position information of structures around the mobile body 1, a classifier 642 that extracts at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure and classifies the extracted points into corresponding areas among the areas in three directions in the coordinate system of the mobile body 1 until the number of points included in each area reaches a specified number, and a comparator 643 that compares the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body 1 moves to estimate the self-position of the mobile body 1.

[0154] According to this configuration, even if the point cloud data of structures in a particular direction of the mobile body 100 becomes extremely small due to the position of the obstacle relative to the mobile body 100, a simple comparison operation reduces the decrease in accuracy of self-position estimation of the mobile body 1 without increasing the calculation load.

[0155] (Aspect 10) The method for controlling a moving body is a method for causing the moving body 1 to perform autonomous movement while estimating the self-position of the moving body 1, and includes the steps of: acquiring three-dimensional point cloud data indicating the position information of structures around the moving body 1; extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure; classifying the extracted points into corresponding areas among three directional areas in the coordinate system of the moving body 1 until the number of points included in each area reaches a specified number; and comparing the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the moving body 1 moves, thereby estimating the self-position of the moving body 1.

[0156] According to this configuration, even if the point cloud data of structures in a particular direction of the mobile body 100 becomes extremely small due to the position of the obstacle relative to the mobile body 100, a simple comparison operation reduces the decrease in accuracy of self-position estimation of the mobile body 1 without increasing the calculation load.

[0157] (Aspect 11) The control program for a mobile body is a control program for a mobile body that causes the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1, and causes a computer to realize the following functions: a function of acquiring three-dimensional point cloud data that indicates the position information of structures around the mobile body 1; a function of extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, and classifying the extracted points into corresponding areas among the areas in three directions in the coordinate system of the mobile body 1 until the number of points included in each area reaches a specified number; and a function of comparing the points classified into each area with points of the three-dimensional point cloud data included in map information regarding a map of the environment in which the mobile body 1 moves, to estimate the self-position of the mobile body 1.

[0158] According to this configuration, even if the point cloud data of structures in a particular direction of the mobile body 100 becomes extremely small due to the position of the obstacle relative to the mobile body 100, a simple comparison operation reduces the decrease in accuracy of self-position estimation of the mobile body 1 without increasing the calculation load. [Explanation of symbols]

[0159] 1 Mobile body 3 sensors 6. Control device 12 Robotic Arm 641 Acquirer 642 classifier 643 Comparator 100 Mobile

Claims

1. A mobile body; a sensor for detecting an object around the mobile body; a control device that causes the mobile body to autonomously move while estimating a self-position of the mobile body based on the detection signal of the sensor, The control device acquiring three-dimensional point cloud data indicating position information of structures around the mobile body from the detection signals of the sensors; extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, and classifying the extracted points into corresponding areas among areas in three directions in a coordinate system of the main body of the moving body until the number of points included in each area reaches a specified number; A mobile body that estimates the self-position of the mobile body by comparing points classified into each region with points of three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body is moving.

2. 2. The moving body according to claim 1, The control device, when acquiring three-dimensional point cloud data of the structure, selects and acquires three-dimensional point cloud data of the structure from three-dimensional point cloud data that indicates three-dimensional position information of objects surrounding the main body of the mobile body.

3. 2. The moving body according to claim 1, The control device classifies the extracted points into each region so that the number of extracted points does not exceed a specified number.

4. 4. The moving body according to claim 3, A mobile body in which the control device terminates the classification of the extracted points into each area when it determines that the total number of points classified into each area has reached a predetermined number.

5. 5. The moving body according to claim 4, The predetermined number is the total number of the prescribed numbers of moving bodies in each area.

6. 5. The moving body according to claim 4, When classifying extracted points into each area, the control device determines that the total number of points extracted up to that point is greater than or equal to a threshold value, and classifies subsequently extracted points into the corresponding area even if the number of points contained in the area corresponding to that point has reached a specified number.

7. 2. The moving body according to claim 1, The sensor is disposed at the bottom of the main body of the moving body.

8. 2. The moving body according to claim 1, The mobile body is a mobile robot, the mobile body including a robot arm.

9. A control device for a moving body that causes a moving body to perform autonomous movement while estimating a self-position of the moving body, an acquirer for acquiring three-dimensional point cloud data indicating position information of structures around the mobile body; a classifier that extracts at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, and classifies the extracted points into corresponding areas among areas in three directions in a coordinate system of the main body of the moving body until the number of points included in each area reaches a specified number; A control device for a mobile body comprising a comparator that compares points classified into each region with points of three-dimensional point cloud data contained in map information regarding a map of the environment in which the mobile body moves to estimate the self-position of the mobile body.

10. A method for controlling a moving body that causes a moving body to autonomously move while estimating a self-position of the moving body, comprising: Acquiring three-dimensional point cloud data indicating position information of structures around the mobile body; extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, and classifying the extracted points into corresponding areas among areas in three directions in the coordinate system of the main body of the moving body until the number of points included in each area reaches a specified number; A method for controlling a moving body, comprising: comparing points classified into each region with points of three-dimensional point cloud data contained in map information relating to a map of the environment in which the moving body moves, thereby estimating the self-position of the moving body.

11. A control program for a moving body for causing a moving body to autonomously move while estimating a self-position of the moving body, comprising: a function of acquiring three-dimensional point cloud data indicating position information of structures around the mobile body; a function of extracting at least one point from each of a plurality of points included in the three-dimensional point cloud data of the structure, and classifying the extracted points into corresponding areas among areas in three directions in the coordinate system of the main body of the moving body until the number of points included in each area reaches a specified number; A control program for a mobile body that causes a computer to realize a function of estimating the self-position of the mobile body by comparing the points classified into each area with points of three-dimensional point cloud data contained in map information regarding a map of the environment in which the mobile body is moving.

Images