Mobile body, mobile body control device, mobile body control method, and mobile body control program

Abstract

A mobile body (100) comprises: a mobile body main body (1); a sensor (3) that detects an object at the periphery of the mobile body main body (1); and a control device (6) that causes the mobile body main body (1) to execute autonomous travel while estimating the self-position of the mobile body main body (1) on the basis of a detection signal of the sensor (3). The control device (6) acquires three-dimensional point cloud data, which indicates position information of a structure at the periphery of the mobile body main body (1), from the detection signal of the sensor (3), extracts at least one point at a time from a plurality of points included in three-dimensional point cloud data of the structure, classifies the extracted points into corresponding regions of a three-dimensional region in a coordinate system of the mobile body main body (1) until the number of points included in each region reaches a prescribed number, and estimates the self-position of the mobile body main body (1) by comparing the points classified into each region with the points of the three-dimensional point cloud data included in map information relating to a map of an environment in which the mobile body main body (1) travels.

Description

Moving body, control device for moving body, control method for moving body, and control program for moving body

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

[0002] Conventionally, moving bodies that move autonomously have been known. For example, Patent Document 1 discloses a moving body that moves autonomously while estimating its own position.

[0003] Japanese Unexamined Patent Application Publication No. 2014 - 195868

[0004] In the moving body as described above, structures such as walls and floors around the moving body are detected by sensors, and the own position is estimated based on the detection signals of the sensors. The moving body acquires point cloud data indicating the position information of the structure from the detection signals of the sensors.

[0005] By the way, the sensor may not be able to detect the structure due to an obstacle near the moving body. In this case, depending on the position of the obstacle, there is a possibility that the point cloud data of the structure in a specific direction among the three directions in the coordinate system of the moving body becomes extremely small. In this case, when estimating the own position by comparing the point cloud data with the map data, an imbalance occurs in the scan matching between the point cloud data and the map data in the three directions of the moving body, and the accuracy of the own position estimation decreases.

[0006] The technology disclosed herein has been made in view of such points, and the object thereof is to reduce the decrease in the accuracy of the own position estimation without increasing the calculation load by simple comparison operations.

[0007] The mobile body disclosed herein comprises a mobile body body, a sensor for detecting objects around the mobile body body, and a control device for causing the mobile body body to perform autonomous movement while estimating the self-position of the mobile body body based on the detection signals of the sensor. The control device acquires three-dimensional point cloud data indicating the position information of structures around the mobile body body from the detection signals of the sensor, 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 regions among three directional regions in the coordinate system of the mobile body body until the number of points in each region reaches a predetermined number, and estimates the self-position of the mobile body body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body body moves.

[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 comprises: an acquirer that acquires three-dimensional point cloud data showing the positional information of structures around 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 structures and classifies the extracted points into corresponding regions among three directional regions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and a comparator that estimates the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

[0009] The method for controlling a mobile body disclosed herein is a method for controlling a mobile body to perform autonomous movement while estimating the self-position of the mobile body, and comprises: acquiring three-dimensional point cloud data showing the positional information of structures surrounding the mobile body; extracting at least one point from a plurality of points included in the three-dimensional point cloud data of the structures, classifying the extracted points into corresponding regions among three directional regions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and estimating the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

[0010] The control program for a mobile body disclosed herein is a control program for a mobile body that causes the mobile body to perform autonomous movement while estimating the self-position of the mobile body, and causes a computer to implement the following functions: a function to acquire three-dimensional point cloud data showing the positional information of structures around the mobile body; a function to extract at least one point from each of the multiple points included in the three-dimensional point cloud data of the structures, classify the extracted points into corresponding regions of three directions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and a function to estimate the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

[0011] According to the aforementioned mobile body, the control device for the mobile body, the control method for the mobile body, and the control program for the mobile body, the decrease in the accuracy of self-position estimation can be reduced without increasing the computational load through simple comparison calculations.

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

[0013] The following describes exemplary embodiments in detail with reference to the drawings. Figure 1 is a perspective view of the mobile body 100. The mobile body 100 performs autonomous movement. The mobile body 100 comprises a mobile body body 1 and a control device 6 that causes the mobile body body 1 to perform autonomous movement. For example, the mobile body 100 moves within a facility such as a store, hospital, or nursing home. In addition to movement, the mobile body 100 may perform tasks such as handing over goods or opening and closing doors.

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

[0015] The trolley 10 has a defined front-to-back direction. In this example, the trolley 10 has a roughly rectangular planar shape. For example, the long side of the rectangle is the front-to-back direction, and the short side of the rectangle is the left-to-right direction.

[0016] The trolley 10 includes multiple wheels 13 and is capable of movement. In this example, the trolley 10 includes four wheels 13. The trolley 10 may be capable of moving in a straight line and turning. In this example, the trolley 10 is capable of moving forward and backward, left and right, and diagonally while maintaining its posture, i.e., it is capable of movement in all directions. In other words, the trolley 10 may be capable of parallel movement in directions other than the forward and backward direction. Furthermore, the trolley 10 may also be capable of rotating in place. For example, the four wheels 13 include a pair of wheels 13 arranged in the left-right direction at the front of the bottom of the trolley 10 and a pair of wheels 13 arranged in the left-right direction at the rear of the bottom of the trolley 10. They may be arranged to form a rectangle at the bottom of the trolley 10. More specifically, the four wheels 13 are arranged at the four corners of the bottom of the trolley 10.

[0017] More specifically, wheel 13 may be an omnidirectional wheel. In this example, wheel 13 is a Mecanum wheel. Wheel 13 has a plurality of barrel-shaped rollers arranged around its outer circumference. For example, the axis of rotation of each roller is inclined at 45 degrees with respect to the axle of wheel 13.

[0018] The mobile body 1 may have a motor 13a for driving the wheels 13 and an encoder 13b for detecting the amount of rotation of the motor 13a (see Figure 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 driven independently by the corresponding motors 13a.

[0019] The trolley 10 may be able to move in any direction in two dimensions using these four wheels 13. For example, the trolley 10 can move or rotate in any direction, such as forward, backward, left, right, or diagonally. The trolley 10 can also rotate in place.

[0020] The base 11 may be mounted on the trolley 10. In this example, the base 11 has a shape that mimics the upper body of a person. The base 11 may be fixed to the trolley 10 so as not to move.

[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. A hand 14 may not be attached to the tip of the other robot arm 12.

[0022] The two robot arms 12 are each connected to different parts of the base 11. For example, the two robot arms 12 are each connected to different parts 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 points of one robot arm 12 to the base 11 and the connection points 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, in the base 11, the front direction is defined as the side facing the front and the back direction as the rear. The width direction may be horizontal and perpendicular to the front-to-back direction. That is, the width direction is the left-to-right direction with respect to the front-to-back 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] Furthermore, if the planar shape of the trolley 10 is a roughly rectangular shape with a longitudinal direction and a transverse direction, the width direction will substantially coincide with the transverse direction of the planar shape of the trolley 10.

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

[0025] For example, the multiple links L include a first link L1, second link L2, third link L3, fourth link L4, fifth link L5, sixth link L6, and seventh link L7, which are arranged in series from the base 11. 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, second joint J2, third joint J3, fourth joint J4, fifth joint J5, sixth joint J6, and seventh joint J7, which are arranged in series from the base 11. The position and orientation of the seventh link L7 have six degrees of freedom, combining the translational and rotational directions for each of the three orthogonal axes. The robot arm 12 may also be a so-called seven-axis robot, having seven joints J. In other words, the robot arm 12 has redundancy. Redundancy is the characteristic that 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 the first joint J1. The first link L1 and the second link L2 are rotatably connected by the second joint J2. The second link L2 and the third link L3 are rotatably connected by the third joint J3. The third link L3 and the fourth link L4 are rotatably connected by the fourth joint J4. The fourth link L4 and the fifth link L5 are rotatably connected by the fifth joint J5. The fifth link L5 and the sixth link L6 are rotatably connected by the sixth joint J6. The sixth link L6 and the seventh link L7 are rotatably connected by the seventh joint J7.

[0027] A hand 14 may be connected to the 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] More specifically, multiple joints J may include joints that function as a shoulder joint. For example, multiple joints J may include joints that have the functions of horizontal extension and horizontal flexion in the shoulder joint. The axis of rotation of joints that have the functions of horizontal extension and horizontal flexion in the shoulder joint extends in a substantially vertical direction. Multiple joints J may include joints that have the functions of extension and flexion in the shoulder joint. The axis of rotation of joints that have 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 the shoulder joint of the robot arm 12. The first joint J1 may also have the functions of horizontal extension and horizontal flexion in the shoulder joint. The axis of rotation of the first joint J1 extends in a substantially vertical direction.

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

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

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

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

[0034] The robot arm 12 has motors 12a (see Figure 3) that rotate each joint J. For example, the motors 12a are servo motors. Each motor 12a has an encoder 12b (see Figure 3).

[0035] The mobile body 100 may be equipped with a sensor 3 that detects objects around the mobile body 1 (hereinafter simply referred to as "surrounding objects"). In this disclosure, "object" includes both inanimate and living things. The sensor 3 is located on the mobile body 1. For example, the sensor 3 is located on a trolley 10 at the bottom of the mobile body 1. The sensor 3 in this example is a distance measuring sensor that measures the distance from the sensor 3 to the surrounding 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 irradiates laser light toward the surroundings of the mobile body 1 and a light-receiving unit that receives the laser light that strikes the surface of the surrounding objects and is reflected. The sensor 3 measures the flight time from the laser light irradiated from the light-emitting unit until it strikes the surface of the surrounding objects and returns to the light-receiving unit. Based on the measured flight time, the sensor 3 measures the distance from the sensor 3 to the surface of the surrounding objects. The sensor 3 may generate point cloud data based on the measured distance. The point cloud data is three-dimensional positional information of the surface of the surrounding objects. For example, sensor 3 outputs the calculated point cloud data to control device 6. Sensor 3 may repeatedly detect surrounding objects at a predetermined detection cycle when the mobile body 1 is moving. Sensor 3 may output the detection result, i.e., point cloud data, to control device 6 each time a surrounding object is detected.

[0036] In this example, the mobile body 100 is equipped with a plurality of sensors 3. Figure 2 is a schematic diagram showing the detection range of the sensors 3. Figure 2 is a plan view of the mobile body 100, and the robot arm 12, etc., are omitted. 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 arranged on the trolley 10. The first sensor 3A is located at the front of the trolley 10. For example, the first sensor 3A is located on the trolley 10 in front 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 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 in the horizontal direction, as shown by the dashed line in Figure 2. In the vertical direction, the first sensor 3A scans the measurement light within a predetermined range that includes the elevation angle and the depression angle.

[0037] The second sensor 3B and the third sensor 3C may be located at the rear of the trolley 10. More specifically, the second sensor 3B and the third sensor 3C are located behind the base 11 of the trolley 10. The second sensor 3B is located at the left rear corner of the trolley 10, and the third sensor 3C is located at the right rear corner of the trolley 10. The second sensor 3B and the third sensor 3C may detect objects in the horizontal two-dimensional space 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 horizontally. The second sensor 3B and the third sensor 3C detect objects in the horizontal range that cannot be detected by at least the first sensor 3A. The second sensor 3B scans the measurement light at least to the left rear of the trolley 10. The third sensor 3C scans the measurement light at least to the right rear of the trolley 10. The scanning range of the measurement light from the second sensor 3B and the scanning range of the measurement light from the third sensor 3C partially overlap at the rear of the trolley 10. In this example, the second sensor 3B scans the measurement light horizontally for approximately 270 degrees from the front to the right, including the area to the left of the mobile body 1, as shown by the dashed line in Figure 2. The third sensor 3C scans the measurement light horizontally for approximately 270 degrees from the front to the left, including the area to the right of the mobile body 1, as shown by the dashed line in Figure 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 from the second sensor 3B and the scanning plane of the measurement light from the third sensor 3C are at approximately the same height.

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

[0039] Hereafter, unless otherwise distinguished, the first sensor 3A, the second sensor 3B, and the third sensor 3C will simply be referred to as "sensor 3".

[0040] Figure 3 shows the hardware configuration of the control device 6. The control device 6 controls the entire mobile body 1. The control device 6 estimates the self-position of the mobile body 1 and causes the mobile body 1 to perform autonomous movement. The control device 6 moves the mobile body 1 by operating the motors 13a of the wheels 13. Furthermore, the control device 6 controls the motors 12a of the robot arm 12 to cause the robot arm 12 to perform predetermined tasks. The control device 6 has a processor 61, a memory 62, and a memory 63.

[0041] The processor 61 performs various calculations. For example, the processor 61 is formed by a processor such as a CPU (Central Processing Unit). The processor 61 may also be formed by an MCU (Micro Controller Unit), MPU (Micro Processor Unit), FPGA (Field Programmable Gate Array), PLC (Programmable Logic Controller), system LSI, etc. The mobile body 1 moves autonomously by the processor 61 operating the motor 13a.

[0042] The memory 62 stores programs and various data executed by the processor 61. For example, the memory 62 stores control programs. The memory 62 also stores map information relating to 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, railings, 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, railings, 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 made of non-volatile memory, an HDD (Hard Disc Drive), or an SSD (Solid State Drive), etc. Memory 63 temporarily stores data, etc. For example, memory 63 is made of volatile memory.

[0043] Figure 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 control programs from the memory 62 into the memory 63 and expanding them. Specifically, the processor 61 functions as a state estimater 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 the path of 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 variable calculator 69 that calculates the operation variable of the motor 13a.

[0044] The state estimator 64 performs self-position estimation. The state estimator 64 receives the detection results from the sensor 3, the detection results from the encoder 13b, and the map information from the memory 62 as input. The map information is, for example, a three-dimensional map. The state estimator 64 compares the detection results from the sensor 3 with the map information to estimate the current position of the mobile body 1, i.e., its own position. Here, the position of the mobile body 1 also includes its orientation, 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 3A. The state estimator 64 compares the environmental information around the mobile body main body 1 obtained from the three-dimensional point cloud data of the first sensor 3A with the three-dimensional map, and estimates the position of the mobile body main body 1 in the environment represented by the three-dimensional map, that is, the self-position.

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

[0047] Further, the map generator 65 updates the two-dimensional map. The update of the two-dimensional map can also be performed during autonomous movement. The map generator 65 detects obstacles in the environment based on the detection result of the sensor 3 acquired during the movement of the mobile body main body 1, and updates the two-dimensional map.

[0048] The path generator 66 reads the destination and map information from the storage 62. The destination is preset in the storage 62. The map information at this time is, for example, a two-dimensional map. At this time, the path generator 66 may also read a via point in addition to the destination. The state quantity (including the estimated position) of the mobile body main body 1 is input to the path generator 66 from the state estimator 64.

[0049] The route generator 66 generates a route from the current position of the mobile body 1 to the destination based on map information. The route generator 66 generates a route that avoids interference with obstacles, etc., by referring to the map information. If a path is established in the environment, the route generator 66 generates a route along the path. For example, the route generator 66 generates a route using the A-star search algorithm, RRT algorithm, Dijkstra's algorithm, or a geometric approach. The route generator 66 outputs an array of positions that the mobile body 1 will pass through as a route 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 for the mobile body 1 from its current position, following the generated path. The trajectory generator 67 generates the target trajectory for the mobile body 1 using a predetermined method (for example, the 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 the command velocity of the mobile body 1.

[0051] Alternatively, the trajectory generator 67 may calculate the command velocity by model predictive control (MPC). Model predictive control obtains the control input, i.e., the velocity 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 any obstacles, calculates the optimal path for the mobile body 1, and calculates the command velocity as the speed at which it will travel from its current position to its target position along that path.

[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 corresponding to the command speed to the manipulated variable calculator 69.

[0053] The mobile controller 68 performs controls to avoid interference between the mobile body 1 and the obstacle. The mobile controller 68 monitors the proximity of the mobile body 1 to the obstacle based on the detection results of the sensors 3. In this example, the mobile controller 68 uses all the detection results from the first sensor 3A, the second sensor 3B, and the third sensor 3C to monitor the proximity of the mobile body 1 to the obstacle. For example, the mobile 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 command values ​​to the multiple motors 13a and calculates the commanded manipulated variable for each of the multiple motors 13a. For example, the manipulated variable may be the rotational speed or torque of the motor.

[0055] Each motor 13a operates according to the commanded input. A motor 13a may be equipped with its own controller for operation. For example, if motor 13a is a servo motor, it further includes a servo amplifier. In that case, the servo amplifier operates motor 13a according to the commanded input. As a result, the mobile body 1 moves.

[0056] Next, the basic operation of the mobile unit 100 will be explained. Figure 5 is a flowchart of the basic operation of the mobile unit 100. The mobile unit 100 repeatedly performs the following processes at a predetermined control cycle.

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

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

[0059] Next, in step S3, the route generator 66 performs route planning. The route generator 66 generates a route for the mobile body 1 based on map information and the estimated position and destination of the mobile body 1.

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

[0061] In step S5, the movement controller 68 causes the mobile body 1 to perform an action according to the commanded speed.

[0062] The mobile unit 100 moves autonomously to its destination while estimating its own position by repeating the above process.

[0063] Next, we will explain in more detail the self-localization of the state estimator 64. Figure 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 comparer 643 as functional blocks.

[0064] The acquisition device 641 acquires three-dimensional point cloud data indicating the positional information of structures around the mobile body 1 from the detection signal of the sensor 3. Structures are immovable building components within a room, such as walls, columns, ceilings, or floors. Structures exclude movable obstacles.

[0065] Specifically, the acquisition device 641, in acquiring three-dimensional point cloud data of a structure, selects and acquires three-dimensional point cloud data of the structure from three-dimensional point cloud data showing the three-dimensional position information of objects around the mobile body 1. More specifically, the acquisition device 641 acquires three-dimensional point cloud data of all objects around the mobile body 1 from the detection signal of the sensor 3, and selects three-dimensional point cloud data of the structure by excluding three-dimensional point cloud data of obstacles and other objects from the three-dimensional point cloud data of all objects.

[0066] The classifier 642 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 (hereinafter also referred to as extracted points) into corresponding regions of the three directional regions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number. The three directional regions 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-and-down direction of the mobile body 1. In this example, the classifier 642 extracts one point from a plurality of points included in the three-dimensional point cloud data of the structure, and classifies the extracted points one by one into the corresponding region of the mobile body 1. Alternatively, the classifier 642 may extract multiple points from a plurality of points included in the three-dimensional point cloud data of the structure, and classify the extracted points multiple times into the corresponding region.

[0067] Specifically, the classifier 642 randomly selects one point from multiple points included in the three-dimensional point cloud data of the structure. Then, the classifier 642 searches for the region corresponding to the selected point and classifies the selected point into the corresponding region. The classifier 642 randomly selects and classifies points one by one until the number of points in each region reaches a predetermined number. The predetermined number is, for example, 67, and is a value determined by parameter adjustment.

[0068] Here, we will explain how the classifier 642 classifies the extracted points into each region. First, the classifier 642 determines 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 the region corresponding to the extracted point and classifies the extracted point into the third region. The predetermined value includes 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. The floor or ceiling is an example of a structure. In this way, the classifier 642 classifies the points included in the three-dimensional point cloud data of the floor or ceiling into the third region.

[0069] If the height of the extracted point is not a predetermined value, the classifier 642 searches for a first or second region as the corresponding region of the extracted point and classifies the extracted point into either the first or second region. Figure 7 is a schematic diagram in plan view showing the relationship between the first region Z1 and the second region Z2 of the mobile body 1 and a point P included in the three-dimensional point cloud data of the wall W, representing the three dimensions in two dimensions for simplicity. In Figure 7, in plan view, the center of gravity of the mobile body 1 is taken as the origin, 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 left 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 shown with hatching.

[0070] The first region Z1 is the region where the absolute value of the X coordinate is greater than the absolute value of the Y coordinate. The region within the first region Z1 where the X coordinate is positive is the preceding region Z11. The region within the first region Z1 where the X coordinate is negative is the following region Z12.

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

[0072] Figure 7 shows the three-dimensional point cloud data of wall W acquired by the acquirer 641. The three-dimensional point cloud data of wall W includes multiple points P. Wall W is an example of a structure. The classifier 642 determines the X and Y coordinates of points P extracted from the multiple points P. If the absolute value of the X coordinate of an extracted point P is greater than the absolute value of the Y coordinate, the classifier 642 searches the first region Z1 as the corresponding region for the extracted point P and classifies the extracted point P into the first region Z1. On the other hand, if the absolute value of the Y coordinate of an 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 for the extracted point P and classifies the extracted point P into the second region Z2.

[0073] In this way, the classifier 642 classifies points P included in the three-dimensional point cloud data of the wall W located in the front-to-back direction of the mobile body 1 into the first region Z1. The classifier 642 also 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 the second region Z2.

[0074] In Figure 7, the classifier 642 determines the X and Y coordinates of the extracted point P and classifies the extracted point P into either the first region Z1 or the second region Z2. However, the extracted point P may be classified into either the first region Z1 or the second region Z2 by other methods. Figure 8 is a schematic diagram illustrating another method for classifying the extracted point P into either the first region Z1 or the second region Z2 of the mobile body 1. Figure 8 corresponds to Figure 7. Figure 8 is a schematic diagram viewed in 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, representing the three dimensions in a simplified two-dimensional representation. In Figure 8, the same symbols as in Figure 7 represent the same configuration, so their explanation is 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 less than 45°, the classifier 642 classifies the extracted point P into the first region Z1. On the other hand, if the angle is greater than 45°, the classifier 642 classifies the extracted point P into the second region Z2.

[0076] In this way, the classifier 642 classifies points P included in the three-dimensional point cloud data of the wall W located in the front-to-back direction of the mobile body 1 into the first region Z1. The classifier 642 also 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 the second region Z2.

[0077] The classifier 642, in classifying extracted points into each region, does not classify extracted points into each region in excess of a specified number. Specifically, when the classifier 642 classifies the extracted predetermined points into a predetermined region corresponding to the predetermined points, if it determines that the number of points included in the predetermined region has reached a specified number, it does not classify the predetermined points into the predetermined region.

[0078] The classifier 642 terminates the classification of extracted points into each region when it determines that the total number of points classified into each region has reached a predetermined number. More specifically, the classifier 642 terminates the classification of 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 specified points for each region. The predetermined number is, for example, 201 (specifically, 67 specified points × 3 = 201), and is a value determined by parameter adjustment. In other words, the classifier 642 terminates the classification of extracted points into each region when it determines that the quantity of points classified into each region has reached a predetermined number. More specifically, the classifier 642 terminates the classification of extracted points into each region when it determines that the quantity 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, the classifier 642 determines that the total number of points extracted up to that point (cumulative number) is equal to or greater than a threshold, and then classifies any subsequently extracted points into the corresponding region, even if the number of points in that region has reached a predetermined number. The threshold is, for example, 10 times a predetermined number (specifically, 201 predetermined points × 10 = 2010 points), and is a value determined by parameter adjustment.

[0080] In more detail, if the regions corresponding to the extracted points are concentrated in one region and the total number of points classified in each region does not reach a predetermined number, the classification of the extracted points into each region will not be completed. Therefore, even if a number of points exceeding the threshold is extracted, if the total number of points classified in each region does not reach a predetermined number, subsequently extracted points will be classified into the corresponding region even if the number of points in that region reaches a predetermined number. As a result, the classification of the extracted points into each region can be completed once the total number of points classified in each region reaches a predetermined number.

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

[0082] The comparator 643 estimates the self-position 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-position 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 by the state estimator 64 (an example of a control method for the moving object 100) will be explained using a flowchart. Figure 9 is a flowchart of the self-position estimation process by the state estimator 64.

[0084] In step S11, the acquisition device 641 performs acquisition processing. Specifically, the acquisition device 641 acquires three-dimensional point cloud data showing the positional information of structures around the mobile body 1.

[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 regions among the three directional regions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number.

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

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

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

[0089] In step S23, the acquisition device 641 removes point cloud data of the moving object 100 from the total point cloud data. Specifically, the acquisition device 641 searches for a second point from all the points included in the total point cloud data such that the distance to the moving object 100 is less than a second setpoint, and removes this second point from all the points. The second setpoint is smaller than the first setpoint and is a numerical value determined, for example, by parameter adjustment.

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

[0091] In step S25, the acquisition device 641 removes point cloud data of a predetermined height from the total point cloud data. Specifically, the acquisition device 641 searches for a fourth point whose height is outside a predetermined range from among all points included in the total 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, including 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, including 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 ceiling height. 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 acquisition device 641 acquires three-dimensional point cloud data of the structure. Specifically, the acquisition device 641 acquires three-dimensional point cloud data of the walls, floors, or ceilings. After that, the process returns to the flowchart in Figure 9.

[0094] Figure 11 is a flowchart of the subroutine for the classification process (step S12). First, in step S31, the classifier 642 randomly selects one point from multiple 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 the region corresponding to the extracted point (hereinafter also referred to as the corresponding region) among the three directional regions in the coordinate system of the mobile body 1. Specifically, as described above 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 for the extracted point.

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

[0097] If the classifier 642 determines that the number of points included in the corresponding area does not reach a predetermined number, in step S36, the classifier 642 classifies the extracted points into the corresponding area. If the classifier 642 determines that the number of points included in the corresponding area has reached a predetermined number, in step S34, the classifier 642 determines whether the total number of extracted points up to the immediate time, excluding the extracted points this time, 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 step is equal to or greater than the threshold, in step S36, the classifier 642 classifies the extracted points into the corresponding area. If the classifier 642 determines that the total number of extracted points up to the previous step is not equal to or greater than the threshold, in step S35, the classifier 642 removes the extracted points without classifying them into the corresponding area.

[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, in step S38, the classifier 642 completes the classification of the extracted points into each region. After that, the process returns to the flowchart in Figure 9.

[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 selects one point from a plurality of points included in the three-dimensional point cloud data of the structure. At this time, the classifier 642 removes the point that was just selected from the plurality of points and selects a new point. Then, the classifier 642 repeats steps S32 to S37.

[0102] Figure 12 is a schematic diagram illustrating the classification process by the classifier 642. Figure 12 corresponds to Figure 7. Figure 12 is a schematic diagram viewed in plan view showing the relationship between the first region Z1 and the second region Z2 of the mobile body 1 and the points P included in the three-dimensional point cloud data of the wall W, representing the three dimensions in a simplified two-dimensional form. In Figure 12, the same symbols as in Figure 7 are the same configuration, so their explanation is omitted. In Figure 12, an obstacle 30 exists 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-back 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-back direction of the mobile body 1 into the first region Z1, and 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 the second region Z2. Specifically, the classifier 642 classifies a predetermined number of points P into the first region Z1 and a predetermined number of points P into the second region Z2. In Figure 12, the points P classified by the classifier 642 are shown enclosed in dashed circles, and the predetermined number for each region is set to 3 for clarity. In this way, even if the number of points P in the Y direction acquired by the acquirer 641 is extremely small compared to the number of points P in the X direction acquired by the acquirer 641, the classifier 642 will classify the same number of points P in the first region Z1 and the second region Z2.

[0104] According to the aforementioned mobile body 100, the control device 6 (specifically, the state estimator 64) acquires three-dimensional point cloud data of structures surrounding the mobile body 1, 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 regions among the three directional regions of the mobile body 1 until the number of points in each region reaches a predetermined number, and estimates the self-position of the mobile body 1 by comparing the points classified into each region with the points in the three-dimensional point cloud data included in the map information.

[0105] As a result, even if the point cloud data of structures in a particular direction of the mobile body 1 becomes extremely small due to the position of obstacles relative to the mobile body 100, the control device 6 classifies points into each region of the mobile body 1 until the number of points in each of the three directional regions of the mobile body 1 reaches a predetermined number, and then compares the points classified into each region with the points in the three-dimensional point cloud data included in the map information. Therefore, the balance between the three directional regions of the mobile body 1 is improved in the scan matching of the point cloud data classified into each region and the three-dimensional point cloud data included in the map information, and the decrease in the accuracy of self-position estimation of the mobile body 1 is reduced without increasing the computational load by simple comparison calculations.

[0106] Furthermore, the control device 6 extracts points one by one from multiple points included in the three-dimensional point cloud data of the structure, and classifies the extracted points one by one into corresponding regions of the three directions until the number of points in each region reaches a predetermined number. This reduces the computational load compared to classifying all points included in the three-dimensional point cloud data of the structure into each region.

[0107] Furthermore, the control device 6 selects and acquires three-dimensional point cloud data of structures from the three-dimensional point cloud data of objects surrounding the mobile body 1, making it easy to acquire three-dimensional point cloud data of structures.

[0108] Furthermore, since the control device 6 does not classify extracted points into each region in excess of a specified number, it can reduce the bias in the quantity of points classified into each region. As a result, the balance between the three directional regions of the mobile body 1 is improved in the scan matching of the point cloud data classified into each region and the three-dimensional point cloud data included in the map information, thereby reducing the decrease in the accuracy of the 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 region has reached a predetermined number, it terminates the classification of extracted points into each region, thereby reducing the bias in the quantity of points classified into each region. As a result, the balance between the three directional regions of the mobile body 1 is improved in the scan matching of the point cloud data classified into each region and the three-dimensional point cloud data included in the map information, thereby reducing the decrease in the accuracy of the self-position estimation of the mobile body 1.

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

[0111] Furthermore, 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, it will classify any points extracted thereafter into the corresponding region, even if the number of points in that region has reached a predetermined number. As a result, the classification of extracted points into each region can be completed once the total number of points classified into each region reaches a predetermined number. Therefore, an increase in computational load can be prevented.

[0112] Furthermore, since the sensor 3 is located at the bottom of the mobile body 1, the sensor 3 may not be able to detect structures due to obstacles near the mobile body 100. However, since the mobile body 100 is equipped with the control device 6 described above, even if the point cloud data of structures in a particular 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 the accuracy of the self-position estimation of the mobile body 1 is mitigated. Therefore, the mobile body 100 of this disclosure is suitable when the sensor 3 is located at the bottom of the mobile body 1.

[0113] Furthermore, the mobile body 1 includes a robotic arm 12 and is a mobile robot. When the mobile body 1 moves within a facility such as a store, hospital, or nursing home, even if the point cloud data of structures in a specific direction of the mobile body 1 acquired by the acquisition device 641 within the facility becomes extremely small, the decrease in the accuracy of the mobile body 1's self-position estimation is mitigated.

[0114] The control device 6 may also control the robot arm 12 when performing autonomous movement. Figure 13 is a functional block diagram showing the configuration of the control system of the processor 61 according to a modified example. The processor 61 may also 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 deforms 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 ​​corresponding to the target shape of the robot arm 12. Based on the command values, the arm controller 611 calculates the command operation amount for each of the multiple motors 12a. For example, the operation amount is the rotational speed or torque of the motor.

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

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

[0119] Figure 14 is a side view of the mobile body 1 when the robot arm 12 is in a traveling configuration. Figure 15 is a top view of the mobile body 1 when the robot arm 12 is in a traveling configuration.

[0120] For example, the robot arm 12 in a mobile configuration is positioned relatively high. For instance, the mobile robot arm 12 bends at an intermediate joint between the shoulder joint and the wrist joint, for example, the fourth joint J4. The portion between the base 11 and the intermediate joint extends diagonally downward and backward from the base 11, while the portion between the intermediate joint and the wrist joint extends forward from the intermediate joint. In other words, the mobile robot arm 12 has a shape in which the intermediate joint is pulled backward and bent at the intermediate joint. As a result, the portion of the robot arm 12 closer to the end effector than the intermediate joint is positioned relatively high. Furthermore, the end effector of the robot arm 12 is positioned relatively far back.

[0121] In its mobile configuration, the robot arm 12 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. Since the robot arm 12 is positioned above the first sensor 3A, it may obstruct a portion of the detection range of the first sensor 3A. The detection results of the first sensor 3A corresponding to the robot arm 12 are treated as invalid. The higher the position of the robot arm 12, the further away the robot arm 12 is from the first sensor 3A. The further the robot arm 12 is from the first sensor 3A, the smaller the area of ​​the detection range of the first sensor 3A tends to be obstructed by the robot arm 12. Therefore, in its mobile configuration, the detection range of the first sensor 3A is relatively large.

[0122] Furthermore, the amount of forward protrusion of the robot arm 12 in its travel shape from the base 11 is relatively small. By reducing the amount of forward protrusion of the robot arm 12, the detection range of the first sensor 3A, specifically the diagonally upward forward area from the first sensor 3A, is expanded.

[0123] The overall width of the robot arm 12 in its mobile configuration, as viewed from above, is relatively small. For example, in the mobile configuration, the second link L2 is located on the outermost side in the width direction. Of the multiple links L, all links other than the second link L2 are positioned further inward in the width direction than the second link L2. By making the overall width of the robot arm 12 in its mobile configuration, as viewed from above, relatively small, the possibility of interference between the robot arm 12 and other objects located in the width direction during movement can be reduced. In addition, the robot arm 12 in its mobile configuration may be positioned in front of the rotation axis of the first joint J1 in the front-rear direction by rotating the first link L1 forward around the rotation axis of the first joint J1. This further reduces the width of the second link L2 of the two robot arms, i.e., the overall width of the robot arm 12 as viewed from above.

[0124] The overall shape of the robot arm 12 in its travel configuration, as seen from a plan view, is contained within the carriage 10 in the front-to-back direction. This reduces the possibility of interference between the robot arm 12 and other objects located in the front-to-back direction during travel.

[0125] Furthermore, the shapes of the two robot arms 12 do not have to be exactly the same in terms of their movement. In other words, the shapes of the two robot arms 12 may be slightly different. For example, the tip of one robot arm 12 may be at a different height than the tip of the other robot arm 12. The seventh joint J7 of one robot arm 12 may be at a different rotation angle than 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 robot arm 12 to move and allows the robot arm 12 to move freely. For example, the arm controller 611 operates the robot arm 12 after the mobile body 1 has reached its destination and performs work with the robot arm 12.

[0127] 《Other Embodiments》 As described above, the embodiments described herein have been presented as examples of the technology disclosed herein. However, the technology in this disclosure is not limited thereto and can be applied to embodiments that have been modified, replaced, added, or omitted as appropriate. It is also possible to combine the components described in the embodiments above to create new embodiments. Furthermore, the components described in the attached 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 illustrate the technology. Therefore, the mere presence of such non-essential components in the attached drawings and detailed description should not be immediately assumed to mean that those non-essential components are essential.

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

[0129] Sensor 3 is not limited to LiDAR. Sensor 3 may be a two-dimensional or three-dimensional camera. Sensor 3 may be a three-dimensional scanner. Sensor 3 may be located on parts of the mobile body 1 other than the trolley 10. The number of sensors 3 is not limited to three. The number of sensors 3 may be one, two, or four or more. The scanning range of the measurement light of the first sensor 3A, second sensor 3B, and third sensor 3C described above is merely an example. For example, the first sensor 3A may scan the measurement light in a region that includes at least the area in front of the mobile body 1. The first sensor 3A may scan the measurement light in 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. The second sensor 3B and the third sensor 3C may each scan the measurement light horizontally 360 degrees. The second sensor 3B may scan the measurement light in a predetermined range (not limited to 270 degrees) that includes the area to the left rear of the mobile body 1. For example, the second sensor 3B may scan the measurement light 360 degrees horizontally. The third sensor 3C may scan the measurement light within a predetermined range (not limited to 270 degrees) that includes the area to the right rear of the mobile body 1. The third sensor 3C may scan the measurement light 360 degrees horizontally.

[0130] In this embodiment, the classifier 642 classifies 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. However, points included in the three-dimensional point cloud data of both the floor and ceiling may be classified into a third region.

[0131] In this embodiment, when classifying extracted points into each region, the classifier 642 determines that the total number of points extracted up to that point is equal to or greater than a threshold, and classifies subsequently extracted points into the corresponding region even if the number of points in that region has reached a predetermined number. However, it is not necessary to classify subsequently extracted points into the corresponding region if the number of points in that region has reached a predetermined number.

[0132] The flowchart is merely an example. You may change, replace, add, or omit steps in the flowchart as needed. You may also change the order of steps in the flowchart or process sequentially in parallel.

[0133] The control method is not limited to the aforementioned control device 6 and can be implemented by other devices. The control program is not limited to the aforementioned control device 6 and can be implemented by other devices.

[0134] The functions 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 circuits. The functions 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 circuits. One or more circuits or processing circuits may be programmed using one or more programs stored together or individually in one or more memories, or may be otherwise configured to perform the disclosed functions. A processor is considered a processing circuit or circuit because it includes transistors and other circuits. A processor may be a programmed processor that executes programs stored in memory. In this disclosure, a circuit, unit, or means is hardware that performs the enumerated functions individually or in combination with each other, or hardware programmed to perform the enumerated functions individually or in combination with each other. The hardware may be any hardware disclosed herein that is programmed or configured to perform the listed functions.

[0135] A computer program, including computer instructions, is stored in memory. The computer instructions provide logic and routines that enable hardware to execute the methods disclosed herein. The hardware includes, for example, processing circuits or circuits. The computer program may be implemented in known formats on computer-readable storage media, computer program products, memory devices, recording media such as CD-ROMs or DVDs, and / or in the memory of FPGAs or ASICs.

[0136] [Embodiments] The embodiments described above are specific examples of the following embodiments.

[0137] (Aspect 1) The mobile body 100 comprises a mobile body 1, a sensor 3 for detecting objects around the mobile body 1, and a control device 6 for causing the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1 based on the detection signals 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 signals 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 regions among the three directional regions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number, and estimates the self-position of the mobile body 1 by comparing the points classified into each region with points in the three-dimensional point cloud data included in map information relating to the map of the environment in which the mobile body 1 moves.

[0138] With this configuration, even if the point cloud data of structures in a particular direction of the moving body 1 becomes extremely small due to the position of obstacles relative to the moving body 100, the decrease in the accuracy of the self-position estimation of the moving body 1 is mitigated by simple comparison calculations without increasing the computational load.

[0139] (Aspect 2) In the mobile body 100 described in Aspect 1, the control device 6, in acquiring the three-dimensional point cloud data of the structure, selects and acquires the three-dimensional point cloud data of the structure from the three-dimensional point cloud data showing the three-dimensional position information of objects around the mobile body 1.

[0140] This configuration allows for easy acquisition of three-dimensional point cloud data of structures.

[0141] (Aspect 3) In the mobile body 100 described in Aspect 1 or Aspect 2, the control device 6 does not classify the extracted points into each region in a specified number of places when classifying the extracted points into each region.

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

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

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

[0145] (Aspect 5) In the mobile body 100 described in any one of aspects 1 to 4, the predetermined number is the total number of the specified numbers in each area.

[0146] With this configuration, the predetermined number is the total number of specified points for each region. Therefore, if the number of points classified into each region reaches the predetermined number, the classification of extracted points into each region can be completed.

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

[0148] This configuration prevents an increase in computational load.

[0149] (Aspect 7) In the mobile body 100 described in any one of aspects 1 to 6, the sensor 3 is located at the lower part of the mobile body 1.

[0150] According to this configuration, the mobile body 100 of this disclosure is preferable when the sensor 3 is located at the bottom of the mobile body 1.

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

[0152] With this configuration, when the mobile body 1 moves within a facility such as a store, hospital, or nursing home, even if the point cloud data of structures in a specific direction within the facility becomes extremely scarce, the decrease in the accuracy of the mobile body 1's self-position estimation is mitigated.

[0153] (Aspect 9) The mobile body control device 6 is a mobile body control device 6 that causes the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1, and comprises an acquirer 641 that acquires three-dimensional point cloud data showing the position information of structures around the mobile body 1, a classifier 642 that extracts at least one point from a plurality of points included in the three-dimensional point cloud data of the structures and classifies the extracted points into corresponding regions of three directions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number, and a comparator 643 that estimates the self-position of the mobile body 1 by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body 1 moves.

[0154] With this configuration, even if the point cloud data of structures in a particular direction of the moving body 1 becomes extremely small due to the position of obstacles relative to the moving body 100, the decrease in the accuracy of the self-position estimation of the moving body 1 is mitigated by simple comparison calculations without increasing the computational load.

[0155] (Aspect 10) A method for controlling a mobile body is a method for controlling a mobile body that causes the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1, and comprises: acquiring three-dimensional point cloud data showing the positional information of structures around the mobile body 1; extracting at least one point from each of the multiple points included in the three-dimensional point cloud data of the structures, classifying the extracted points into corresponding regions among the three directional regions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number; and estimating the self-position of the mobile body 1 by comparing the points classified into each region with points in the three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body 1 moves.

[0156] With this configuration, even if the point cloud data of structures in a particular direction of the moving body 1 becomes extremely small due to the position of obstacles relative to the moving body 100, the decrease in the accuracy of the self-position estimation of the moving body 1 is mitigated by simple comparison calculations without increasing the computational load.

[0157] (Aspect 11) The control program for the mobile body is a control program for the mobile body that causes the mobile body 1 to perform autonomous movement while estimating the self-position of the mobile body 1, and causes the computer to implement the following functions: a function to acquire three-dimensional point cloud data showing the positional information of structures around the mobile body 1; a function to extract at least one point from each of the multiple points included in the three-dimensional point cloud data of the structures, and classify the extracted points into corresponding regions of three directions in the coordinate system of the mobile body 1 until the number of points in each region reaches a predetermined number; and a function to estimate the self-position of the mobile body 1 by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body 1 moves.

[0158] With this configuration, even if the point cloud data of structures in a particular direction of the moving body 1 becomes extremely small due to the position of obstacles relative to the moving body 100, the decrease in the accuracy of the self-position estimation of the moving body 1 is mitigated by simple comparison calculations without increasing the computational load.

[0159] 1 Mobile body 3 Sensors 6 Control device 12 Robot arm 641 Acquirer 642 Classifier 643 Comparator 100 Mobile body

Claims

1. A mobile body comprising: a mobile body; a sensor for detecting objects around the mobile body; and a control device for causing the mobile body to perform autonomous movement while estimating the self-position of the mobile body based on the detection signals 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 signals of the sensor; 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 regions among three directional regions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and estimates the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

2. The mobile body according to claim 1, wherein the control device, in 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 indicating three-dimensional position information of objects surrounding the mobile body.

3. The mobile body according to claim 1, wherein the control device does not classify the extracted points into each region in a specified number of ways in the classification of the extracted points into each region.

4. The mobile body according to claim 3, wherein the control device determines that the total number of points classified into each region has reached a predetermined number in the classification of extracted points into each region, and the mobile body terminates the classification of extracted points into each region.

5. A mobile body according to claim 4, wherein the predetermined number is the total number of predetermined numbers in each region.

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

7. The mobile body according to claim 1, wherein the sensor is located at the lower part of the mobile body body.

8. The mobile body according to claim 1, wherein the mobile body body includes a robot arm and is a mobile robot.

9. A control device for a mobile body that causes the mobile body to perform autonomous movement while estimating the self-position of the mobile body, comprising: an acquirer that acquires three-dimensional point cloud data showing the positional information of structures around 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 structures and classifies the extracted points into corresponding regions of three directions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and a comparator that estimates the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

10. A method for controlling a mobile body to perform autonomous movement while estimating the self-position of the mobile body, comprising: acquiring three-dimensional point cloud data showing the positional information of structures surrounding the mobile body; extracting at least one point from a plurality of points included in the three-dimensional point cloud data of the structures, classifying the extracted points into corresponding regions among three directional regions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and estimating the self-position of the mobile body by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

11. A control program for a mobile body that causes the mobile body to perform autonomous movement while estimating the mobile body's own position, the program causing the computer to implement the following functions: a function to acquire three-dimensional point cloud data showing the positional information of structures around the mobile body; a function to extract at least one point from a plurality of points included in the three-dimensional point cloud data of the structures, and classify the extracted points into corresponding regions of three directions in the coordinate system of the mobile body until the number of points in each region reaches a predetermined number; and a function to estimate the mobile body's own position by comparing the points classified into each region with points in three-dimensional point cloud data included in map information relating to a map of the environment in which the mobile body moves.

Images