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

Abstract

The mobile body 100 of the present invention includes: a mobile body 1; a sensor 3 for detecting objects around the mobile body 1; and a control device 6 for estimating the self-position of the mobile body 1 based on the detection signals of the sensor 3 and for causing the mobile body 1 to perform autonomous movement. The control device 6 obtains three-dimensional point group data representing the position information of structures around the mobile body 1 from the detection signals of the sensor 3; sequentially extracts at least one point from the plurality of points contained in the three-dimensional point group data of the structures, and classifies the extracted point into a corresponding region among the three directions of the coordinate system of the mobile body 1, until the number of points contained in each region reaches a predetermined number; and compares the points classified into each region with the points in the three-dimensional point group data contained in the map information of a map relating to the environment in which the mobile body 1 moves, thereby estimating the self-position of the mobile body 1.

Description

Technical Field

[0001] This invention relates to the technology of a moving body, a control device for a moving body, a control method for a moving body, and a control program for a moving body. Prior Technology

[0002] To date, autonomously moving bodies are known. For example, Patent Document 1 discloses a moving body that performs self-position estimation while moving autonomously. [Previous Technical Documents] [Patent Literature]

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

[0004] As described above, the mobile system uses sensors to detect structures such as walls and floors around the moving body and infers its own position based on the sensor's detection signals. The mobile system obtains point data representing the position information of the structures from the sensor's detection signals.

[0005] The sensors of this type of moving object may sometimes fail to detect structures due to obstacles located near the moving object. In this case, there is a concern that the location of the obstacle may result in very little point data for structures in a specific direction within the three directions of the moving object's coordinate system. Consequently, when comparing point data with map data to estimate its own position, the scanning match between the point data and map data may create an imbalance between the three directions of the moving object, leading to a decrease in the accuracy of its self-position estimation.

[0006] The technology disclosed in this invention was made in view of this, and its purpose is to reduce the decrease in accuracy of self-position estimation by simply performing comparison operations without increasing the computational load.

[0007] The mobility system of the present invention includes: a mobile body; a sensor for detecting objects around the mobile body; and a control device for estimating the self-position of the mobile body based on the detection signals of the sensor and enabling the mobile body to perform autonomous movement. The control device acquires three-dimensional point group data representing the position information of structures around the mobile body from the detection signals of the sensor; sequentially extracts at least one point from a plurality of points contained in the three-dimensional point group data of the structures, and classifies the extracted point into corresponding regions in three directions of the coordinate system of the mobile body until the number of points contained in each region reaches a predetermined number; and compares the points classified into each region with points in the three-dimensional point group data contained in map information related to the environment in which the mobile body moves, thereby estimating the self-position of the mobile body.

[0008] The control device for a mobile body of the present invention is a control device for a mobile body that estimates the self-position of the mobile body and enables the mobile body to perform autonomous movement. The control device includes: a device for acquiring three-dimensional point group data representing the position information of structures surrounding the mobile body; a classifier that sequentially extracts at least one point from a plurality of points contained in the three-dimensional point group data of the structures and classifies the extracted point into corresponding regions in three directions of the coordinate system of the mobile body until the number of points contained in each region reaches a predetermined number; and a comparator that compares the points classified into each region with points in the three-dimensional point group data contained in map information related to the environment in which the mobile body moves, and estimates the self-position of the mobile body.

[0009] The mobile body control method of the present invention is a mobile body control method that estimates the self-position of the mobile body and enables the mobile body to perform autonomous movement. The control method includes the following steps: obtaining three-dimensional point group data representing the position information of the structures surrounding the mobile body; successively extracting at least one point from the plurality of points contained in the three-dimensional point group data of the structures, and classifying the extracted point into corresponding regions in the three directions of the coordinate system of the mobile body until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with the points in the three-dimensional point group data contained in the map information of the map of the environment in which the mobile body moves, thereby estimating the self-position of the mobile body.

[0010] The control program for the mobile body of the present invention is a control program for a mobile body that estimates its own position and enables the mobile body to perform autonomous movement. The control program enables a computer to perform the following functions: acquiring three-dimensional point data representing the position information of structures surrounding the mobile body; sequentially extracting at least one point from the plurality of points contained in the three-dimensional point data of the structures and classifying the extracted point into corresponding regions in the three directions of the coordinate system of the mobile body until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with the points in the three-dimensional point data contained in the map information of a map relating to the environment in which the mobile body is moving, thereby estimating the own position of the mobile body.

[0011] Based on the mobile body, the control device of the mobile body, the control method of the mobile body, and the control program of the mobile body, the reduction in the accuracy of self-position estimation can be mitigated by simple comparison calculation without increasing the computational load. Simple Explanation of the Diagram

[0012] Figure 1 is a three-dimensional view of the moving body. Figure 2 is a schematic diagram showing the detection range of the sensor. Figure 3 shows 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 movements of the moving body. Figure 6 shows the detailed functional block diagram of the system state estimator. Figure 7 is a schematic diagram showing the relationship between the points in the three-dimensional point group data of the first and second regions of the moving body and the wall, viewed from above. Figure 8 is a schematic diagram illustrating other methods for classifying extraction points into the first or second region of the moving body. Figure 9 shows a flowchart of the self-position estimation process performed by the state estimator. Figure 10 is a flowchart of the sub-procedure for obtaining processing. Figure 11 is a flowchart of the sub-program for classification processing. Figure 12 is a schematic diagram illustrating the classification process performed by a classifier. Figure 13 is a functional block diagram showing the configuration of the control system of the processor in the modified example. Figure 14 shows a side view of the moving body of the robotic arm when it is in a walking shape. Figure 15 is a top view of the moving body of the robotic arm when it is in a walking shape. Implementation

[0013] The following is a detailed description of the illustrated embodiments based on the figures. Figure 1 is a perspective view of the mobile body 100. The mobile body 100 moves autonomously. The mobile body 100 includes a mobile body 1 and a control device 6 that enables the mobile body 1 to perform autonomous movement. For example, the mobile body 100 moves within facilities such as shops, hospitals, or care centers. In addition to movement, the mobile body 100 can also perform operations such as receiving and sending goods or opening and closing doors.

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

[0015] The 10-series trolleys are defined by their front-to-back orientation. In this example, the 10-series trolleys have a generally rectangular planar shape. For example, the long side of the rectangle is in the front-to-back direction. The short side of the rectangle is in the left-to-right direction.

[0016] The trolley 10 is capable of movement by including a plurality of wheels 13. In this example, the trolley 10 includes four wheels 13. The trolley 10 can move in a straight line and turn. In this example, the trolley 10 can move forward, backward, left, right, and diagonally while maintaining its posture, that is, it can move in all directions. Thus, the trolley 10 can also move in directions other than forward and backward. Furthermore, the trolley 10 can also rotate in place. For example, the four wheels 13 may include a set of wheels 13 arranged side by side in the left and right directions on the front side of the bottom of the trolley 10 and a set of wheels 13 arranged side by side in the left and right directions on the rear side of the bottom of the trolley 10, or they may be arranged in a quadrilateral shape on 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] In detail, wheel 13 can be an omnidirectional wheel. In this example, wheel 13 is a Mecanum wheel. Wheel 13 has a plurality of barrel-shaped rollers arranged on its outer circumference. For example, the axis of rotation of each roller is inclined at 45 degrees relative to the axle of wheel 13.

[0018] The mobile body 1 may include a motor 13a that drives the wheels 13 and an encoder 13b that detects the rotation of the motor 13a (see Figure 3). In this example, the mobile body 1 includes four sets of motors 13a and four sets of encoders 13b to correspond to the four wheels 13. The four wheels 13 can be driven independently by their respective motors 13a.

[0019] The trolley 10 can also move in any two-dimensional direction via its four wheels 13. For example, the trolley 10 can move or turn in any direction parallel to forward, backward, left, right, or diagonal. The trolley 10 can also rotate in place.

[0020] The base 11 can be mounted on the trolley 10. In this example, the base 11 has a shape that mimics the human upper body. The base 11 can also be fixed to the trolley 10 without being moved.

[0021] The mobile body 1 includes two robotic arms 12. A hand 14 can be installed at the front end of one robotic arm 12, while a hand 14 is not installed at the front end of the other robotic arm 12.

[0022] The two robotic arms 12 are respectively connected to different parts of the base 11. For example, the two robotic arms 12 are respectively connected to different parts of the base 11 in the width direction when viewed from above. In other words, when viewed from above, the connection direction of one robotic arm 12 and the connection direction of the other robotic arm 12 of the base 11 is the width direction. The base 11 may also include a front and a back side that face opposite each other when viewed from above. For example, in the base 11, the side facing the front is designated as the front side, and the side facing the back is designated as the rear side, to define the front-back direction. The width direction is a direction that extends horizontally and is perpendicular to the front-back direction. That is, the width direction is the left-right direction relative to the front-back direction. For example, one robotic arm 12 is connected to the left side of the base 11, and the other robotic arm 12 is connected to the right side of the base 11.

[0023] Furthermore, when the planar shape of the trolley 10 is a slightly square shape with both a long side and a short side, the width direction is approximately consistent with the short side direction of the planar shape of the trolley 10.

[0024] For example, as shown in Figure 1, the robotic arm 12 includes a plurality of links L and a plurality of joints J connecting the plurality of links L. The robotic arm 12 is configured to perform three-dimensional movements. In this example, the robotic arm 12 is a multi-joint robotic arm. That is, the robotic arm 12 can freely change shape by rotating its joints. The robotic arm 12 is supported by a base 11.

[0025] For example, a plurality of 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, arranged sequentially from the base 11. The seventh link L7 is located at the front end of the robotic arm 12. For example, a plurality of 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, arranged sequentially from the base 11. The position and orientation of the seventh link L7 have a total of six degrees of freedom with respect to the translational and rotational directions of the individual orthogonal three axes. The robotic arm 12 can be a so-called seven-axis robot containing seven joints J. That is, the robotic arm 12 has redundancy. The so-called redundancy refers to the characteristic of not defining the rotation angle of the plurality of joints J corresponding to the position and orientation of the front end of the robotic arm 12 to a single angle.

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

[0027] The seventh link L7 at the front end of the robotic arm 12 can be connected to the hand 14. That is, the hand 14 is rotatably connected to the robotic arm 12 about the rotation axis of the seventh joint J7. The hand 14 is mounted on the end effector of the robotic arm 12.

[0028] In detail, the plurality of joints J may include joints that function as shoulder joints. For example, the joints included in the plurality of joints J may also be joints that have the function of horizontal extension and horizontal flexion of the shoulder joint. The rotational axis system of joints that have the function of horizontal extension and horizontal flexion of the shoulder joint extends generally in the vertical direction. The joints included in the plurality of joints J may also have the function of extension and flexion of the shoulder joint. The rotational axis system of joints that have the function of extension and flexion of the shoulder joint extends generally in the horizontal direction.

[0029] For example, the first joint J1 functions as the shoulder joint of the robotic arm 12. The first joint J1 can have the functions of horizontal extension and horizontal flexion of the shoulder joint. The rotation axis of the first joint J1 extends approximately in the vertical direction.

[0030] For example, the second joint J2 functions as the shoulder joint of the robotic arm 12. The second joint J2 can have the functions of shoulder extension and flexion. The rotation axis of the second joint J2 extends approximately horizontally.

[0031] For example, the third joint J3 functions as the shoulder joint of the robotic arm 12. The third joint J3 can have the functions of internal rotation and external rotation of the shoulder joint.

[0032] The plurality of joints J may also include joints that function as wrist joints. For example, the joints included in the plurality of joints J may also have the functions of internal and external rotation or inversion and eversion of the wrist joint. For example, the seventh joint J7 may have the functions of internal and external rotation of the wrist joint, and the sixth joint J6 may have the functions of inversion and eversion of the wrist joint.

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

[0034] The robotic arm 12 includes motors 12a (see Figure 3) that drive the rotation of each joint J. For example, motors 12a are servo motors. Each motor 12a includes an encoder 12b (see Figure 3).

[0035] The mobile body 100 may also include a sensor 3 for detecting objects around the mobile body 1 (hereinafter referred to simply as "surrounding objects"). Furthermore, in this invention, "object" includes both inanimate and living entities. The sensor 3 is disposed on the mobile body 1. For example, the sensor 3 is disposed on the trolley 10 at the lower part of the mobile body 1. In this example, the sensor 3 is a ranging sensor that measures the distance from the sensor 3 to the surrounding objects. For example, the sensor 3 is a light-detecting and ranging (LiDAR) sensor. The sensor 3, for example, includes a light-emitting part that illuminates laser light around the mobile body 1 and a light-receiving part that receives laser light reflected from the surface of the surrounding objects. The sensor 3 measures the travel time from the laser light illuminating the light-emitting part to the surface of the surrounding objects and back to the light-receiving part. The sensor 3 measures the distance between the sensor 3 and the surface of the surrounding objects based on the measured travel time. The sensor 3 may also generate point cluster data based on the measured distance. The point group data is the three-dimensional position information of the surface of the surrounding objects. For example, sensor 3 outputs the calculated point group data to control device 6. Sensor 3 can also repeatedly detect surrounding objects at a predetermined detection cycle when the moving body 1 moves. Sensor 3 can also output the detection result, i.e., the point group data, to control device 6 each time a surrounding object is detected.

[0036] In this example, the mobile body 100 includes a plurality of sensors 3. Figure 2 is a schematic diagram showing the detection range of the sensors 3. Figure 2 is a top view of the mobile body 100, omitting the robotic arm 12, etc. The mobile body 100 may include 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 trolley 10. The first sensor 3A is disposed at the front of the trolley 10. For example, the first sensor 3A is disposed in front of the base 11 of the trolley 10 and approximately at the center in the left-right direction. The first sensor 3A detects objects in the three-dimensional space surrounding the mobile body 1. The first sensor 3A may be a three-dimensional LiDAR. The first sensor 3A scans in the horizontal and vertical directions with measuring light. In this example, as shown by the two-point chain in Figure 2, the first sensor 3A scans 360 degrees in the horizontal direction with measuring light. In the vertical direction, the first sensor 3A scans with measuring light within a predetermined range including elevation and depression angles.

[0037] The second sensor 3B and the third sensor 3C can be disposed at the rear of the trolley 10. More specifically, the second sensor 3B and the third sensor 3C are disposed behind the base 11 of the trolley 10. The second sensor 3B is disposed at the left rear corner of the trolley 10, and the third sensor 3C is disposed at the right rear corner of the trolley 10. The second sensor 3B and the third sensor 3C can also detect objects in the horizontal two-dimensional space around the moving body 1. For example, the second sensor 3B and the third sensor 3C are two-dimensional LiDARs. The second sensor 3B and the third sensor 3C scan in the horizontal direction with measuring light. The second sensor 3B and the third sensor 3C can detect objects in the horizontal direction that cannot be detected by the first sensor 3A. The second sensor 3B scans at least to the left rear of the trolley 10 with measuring light. The third sensor 3C scans at least to the right rear of the trolley 10 with measuring light. The scanning ranges of the measuring light from the second sensor 3B and the third sensor 3C partially overlap at the rear of the trolley 10. In this example, as shown by the dotted chain line in Figure 2, the second sensor 3B scans approximately 270 degrees horizontally with its measuring light across the region from the front to the right, including the left side of the moving body 1. The third sensor 3C, as shown by the dashed line in Figure 2, scans approximately 270 degrees horizontally with its measuring light across the region from the front to the left, including the right side of the moving body 1. The second sensor 3B and the third sensor 3C detect objects at approximately the same height. That is, the scanning plane of the measuring light from the second sensor 3B and the scanning plane of the measuring 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 range F covering the base 11 with measurement light. In contrast, 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 range F with measurement light.

[0039] Hereinafter, without distinguishing between the first sensor 3A, the second sensor 3B, and the third sensor 3C, they will be collectively 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 enables the mobile body 1 to perform autonomous movement. The control device 6 moves the mobile body 1 by actuating the motor 13a of the wheel 13. Furthermore, the control device 6 controls the motor 12a of the robotic arm 12 to enable the robotic arm 12 to perform predetermined tasks. The control device 6 includes a processor 61, a storage unit 62, and a memory unit 63.

[0041] The processor 61 performs various computational operations. For example, the processor 61 may be formed from a processor such as a Central Processing Unit (CPU). The processor 61 may also be formed from a Microcontroller Unit (MCU), a Microprocessor Unit (MPU), a Field Programmable Gate Array (FPGA), a Programmable Logic Controller (PLC), a system LSI, etc. The processor 61 operates the motor 13a, thereby enabling the moving body 1 to move autonomously.

[0042] Storage 62 stores programs and various data to be executed by processor 61. For example, storage 62 stores control programs. Storage 62 stores map information related to the map of the environment in which the mobile body 1 moves. For example, the map information includes 3D maps and 2D maps. The 3D map is formed from 3D point data. For example, a 3D map is a 3D point map. In the 3D map, the 3D shape of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, is represented by point data. The 2D map is a planar map. A 2D map is, for example, a 2D grid map. In the 2D map, the planar shape of obstacles in the environment, such as walls, ceilings, handrails, shelves, tables, or chairs, is represented. For example, a 2D map is formed by projecting a 3D map onto a plane. Storage 62 is formed from non-volatile memory, hard disk drives (HDDs), or solid state drives (SSDs). Memory 63 temporarily stores data, etc. For example, memory 63 is formed from volatile memory.

[0043] Figure 4 is a functional block diagram showing the configuration of the control system of processor 61. Processor 61 reads the control program from storage 62 and expands it into memory 63 to implement various functions. Specifically, processor 61 functions as a state estimator 64, a map generator 65, a path generator 66, a track generator 67, and a movement controller 68. State estimator 64 estimates the state of the mobile body 1. Map generator 65 generates a map of the environment in which the mobile body 1 moves. Path generator 66 plans the path of the mobile body 1. Track generator 67 generates a target track according to the path. Movement controller 68 moves the mobile body 1 according to the target track. Processor 61 also functions as an operand arithmetic unit 69 for the operation of the operational motor 13a.

[0044] The state estimator 64 performs self-position estimation. The state estimator 64 receives input from the detection results of sensor 3, the detection results of encoder 13b, and map information from storage 62. The map information is, for example, a 3D map. The state estimator 64 compares the detection results of sensor 3 with the map information to estimate the current position of the moving body 1, i.e., its self-position. Here, the position of the moving body 1 also includes its orientation, i.e., its posture.

[0045] In this example, the state estimator 64 performs self-position estimation using the three-dimensional point group data of the first sensor 3A. The state estimator 64 compares the environmental information surrounding the moving body 1 obtained from the three-dimensional point group data of the first sensor 3A with the three-dimensional map to estimate the position of the moving body 1 within the environment represented by the three-dimensional map, which is also its self-position.

[0046] Map generator 65 generates a map based on the detection results of sensor 3. Specifically, map generator 65 generates or corrects a 3D map based on the detection results of sensor 3. In this example, before autonomous movement, a 3D map is generated using Simultaneous Localization and Mapping (SLAM) technology. Specifically, during the movement of the mobile body 1 within the environment, state inferrer 64 and map generator 65 obtain the detection results from sensor 3 and perform self-position inference and map generation in parallel. The generated map information, i.e., the 3D map, is stored in storage 62. When generating a map before autonomous movement, the movement of the mobile body 1 is achieved through manual manipulation by the user.

[0047] Furthermore, the map generator 65 updates the two-dimensional map. The updating of the two-dimensional map can be performed during autonomous movement. The map generator 65 detects obstacles in the environment and updates the two-dimensional map based on the detection results obtained by the sensor 3 during the movement of the moving body 1.

[0048] The route generator 66 reads 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, in addition to reading the destination, the route generator 66 can also read the route. The route generator 66 accepts the state quantity (including the estimated position) of the moving body 1 input from the state estimator 64.

[0049] Path generator 66 generates a path from the current position of the moving body 1 to its destination based on map information. Path generator 66 also generates paths that avoid obstacles and other interference, referencing map information. If there are passageways in the environment, path generator 66 generates paths along those passageways. For example, path generator 66 generates paths using A-star search algorithms, RRT algorithms, Dijkstra's algorithm, or geometric methods. Path generator 66 outputs the arrangement of positions that the moving body 1 will traverse to track generator 67 as the path. Each position includes not only location information but also the pose of the moving body 1.

[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 (e.g., line-of-sight guidance law). The trajectory generator 67 accepts the state variables of the mobile body 1 input from the state estimator 64. The trajectory generator 67 calculates the commanded speed of the mobile body 1.

[0051] Alternatively, the trajectory generator 67 can also calculate the command speed using Model Predictive Control (MPC). MPC solves the optimization problem by iteratively solving the model of the moving body 1 to obtain the control input, i.e., the speed command. The trajectory generator 67 predicts future state variables from the current state variables of the moving body 1 and the obstacles, calculates the optimal path for the moving body 1, and calculates the speed required to move from the current position to the target position to follow that path as the command speed.

[0052] The motion controller 68 receives the command speed calculated by the track generator 67 as input. The motion controller 68 outputs the command value corresponding to the command speed to the operational quantity arithmetic unit 69.

[0053] The motion controller 68 performs controls to avoid interference between the moving body 1 and obstacles. The motion controller 68 monitors the approach of the moving body 1 to the obstacle based on the detection results of the sensor 3. In this example, the motion controller 68 monitors the approach of the moving body 1 to the obstacle using all the detection results of the first sensor 3A, the second sensor 3B, and the third sensor 3C. For example, the motion controller 68 decelerates or stops the moving body 1 based on the distance between the moving body 1 and the obstacle.

[0054] The operation quantity arithmetic unit 69 assigns instruction values ​​to a plurality of motors 13a and calculates the individual instruction operation quantities of the plurality of motors 13a. For example, the operation quantity is the rotational speed or torque of the motor.

[0055] Each motor 13a operates according to a command input. There are also cases where a built-in controller is provided for each motor 13a to operate. For example, when motor 13a is a servo motor, it also has a servo amplifier. In this case, the servo amplifier operates motor 13a according to the command input. As a result, the moving body 1 can move.

[0056] Next, the basic operations of the moving body 100 will be described. Figure 5 is a flowchart of the basic operations of the moving body 100. The moving body 100 repeatedly performs the following processes at predetermined control cycles.

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

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

[0059] Next, in step S3, the path generator 66 executes the path planning. The path generator 66 generates the path of the mobile body 1 based on map information, the estimated location of the mobile body 1, and the destination.

[0060] In step S4, the trajectory generator 67 calculates the command speed from the estimated position of the moving body 1, so that the moving body 1 follows the generated path.

[0061] In step S5, the motion controller 68 causes the moving body 1 to move at the commanded speed.

[0062] The mobile body 100 autonomously moves to its destination while simultaneously estimating its own position by repeatedly performing the above processes.

[0063] Next, the self-position estimation of the state estimator 64 will be described in further detail. Figure 6 is a detailed functional block diagram of the state estimator 64. The state estimator 64 includes a acquirer 641, a classifier 642, and a comparator 643 as functional blocks.

[0064] The acquisition device 641 acquires three-dimensional point data representing the position information of structures surrounding the moving body 1 from the detection signal of the sensor 3. The structures are indoor, non-moving buildings, such as walls, columns, ceilings, or floors. The structures do not contain movable obstacles.

[0065] Specifically, in acquiring the three-dimensional point group data of the structure, the acquirer 641 selects and acquires the three-dimensional point group data of the structure from the three-dimensional point group data representing the three-dimensional position information of objects surrounding the moving body 1. More specifically, the acquirer 641 acquires the three-dimensional point group data of all objects surrounding the moving body 1 from the detection signal of the sensor 3, and selects the three-dimensional point group data of the structure by excluding the three-dimensional point group data of obstacles, etc., from the three-dimensional point group data of all objects.

[0066] The classifier 642 sequentially extracts at least one point from the multiple points contained in the three-dimensional point group data of the structure, and classifies the extracted point (hereinafter also referred to as the extracted point) into the corresponding region in the three-directional regions of the coordinate system of the moving body 1, until the number of points contained in each region reaches a predetermined number. The three-directional regions of the moving body 1 include: a first region in the front-back direction of the moving body 1, a second region in the left-right direction of the moving body 1, and a third region in the up-down direction of the moving body 1. In this example, the classifier 642 sequentially extracts one point from the multiple points contained in the three-dimensional point group data of the structure, and sequentially classifies the extracted point into the corresponding region of the moving body 1. Alternatively, the classifier 642 can sequentially extract multiple points from the multiple points contained in the three-dimensional point group data of the structure, and sequentially classify the multiple extracted points into the corresponding regions.

[0067] Specifically, classifier 642 randomly selects one point from the complex number of points contained in the three-dimensional point group data of the structure. Furthermore, classifier 642 explores the region corresponding to the selected point and classifies the selected point into the corresponding region. Classifier 642 randomly selects and classifies points sequentially until the number of points contained in each region reaches a predetermined number. The predetermined number is, for example, 67 points, which is a value set by parameter adjustment.

[0068] This section describes the classification method for extracted points in each region using classifier 642. First, classifier 642 calculates the height of the extracted points. When the height of the extracted point is a predetermined value, classifier 642 explores a third region as the corresponding region for 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, for example, including an error of ±10cm. The second predetermined value is the height of the ceiling, for example, including an error of ±10cm. The floor or ceiling is an example of a structure. Thus, classifier 642 classifies the points contained in the three-dimensional point group data of the floor or ceiling into the third region.

[0069] When the height of the extraction point is not a predetermined value, classifier 642 explores the first or second region as the corresponding region for the extraction point and classifies the extraction point into the first or second region. Figure 7 is a schematic diagram showing the relationship between the first region Z1 and the second region Z2 of the moving body 1 and the point P contained in the three-dimensional point group data of the wall W under top-down view, which simplifies the three-dimensional representation to two-dimensional. In Figure 7, under top-down view, the center of gravity of the moving body 1 is taken as the origin, the front-back direction of the moving body 1 is defined as the X-axis, and the left-right direction of the moving body 1 is defined as the Y-axis. The front direction of the moving body 1 is taken as the positive direction of the X-axis, and the left direction of the moving body 1 is taken as the positive direction of the Y-axis. The first region Z1 and the second region Z2 are shown with shaded lines.

[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. Within the first region Z1, the region with a positive X coordinate is the front region Z11, and the region with a negative X coordinate is the back 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. Within the second region Z2, the region with a positive Y coordinate is the left region Z21, and the region with a negative Y coordinate is the right region Z22.

[0072] Figure 7 shows the three-dimensional point group data of the wall W obtained by the acquisition unit 641. The three-dimensional point group data of the wall W includes a plurality of points P. The wall W is an example of a structure. The classifier 642 calculates the X and Y coordinates of the point P extracted from the plurality of points P. When 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 explores a first region Z1 as the corresponding region of the extracted point P and classifies the extracted point P into the first region Z1. On the other hand, when 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 explores a second region Z2 as the corresponding region of the extracted point P and classifies the extracted point P into the second region Z2.

[0073] Thus, classifier 642 classifies point P contained in the three-dimensional point group data of the wall W located in the front-back direction of the moving body 1 into the first region Z1. Furthermore, classifier 642 classifies point P contained in the three-dimensional point group data of the wall W located in the left-right direction of the moving body 1 into the second region Z2.

[0074] As shown in Figure 7, classifier 642 calculates the X and Y coordinates of the extracted point P and classifies it into either the first region Z1 or the second region Z2. However, other methods can also be used to classify the extracted point P into either the first region Z1 or the second region Z2. Figure 8 is a schematic diagram illustrating other methods for classifying the extracted point P into the first region Z1 or the second region Z2 of the moving body 1. Figure 8 corresponds to Figure 7. Figure 8 is a schematic diagram showing the relationship between the first region Z1 and the second region Z2 of the moving body 1 and the point P contained in the three-dimensional point group data of the wall W under top-down observation, which simplifies the three-dimensional representation to a two-dimensional representation. In Figure 8, the same symbols as in Figure 7 have the same structure, so their explanation is omitted.

[0075] The classifier 642 calculates the angle between the normal direction N of the extracted point P and the X-axis. When the angle is less than 45°, the classifier 642 classifies the extracted point P into the first region Z1. On the other hand, when the angle is greater than 45°, the classifier 642 classifies the extracted point P into the second region Z2.

[0076] Thus, classifier 642 classifies point P contained in the three-dimensional point group data of the wall W located in the front-back direction of the moving body 1 into the first region Z1. Furthermore, classifier 642 classifies point P contained in the three-dimensional point group data of the wall W located in the left-right direction of the moving body 1 into the second region Z2.

[0077] In classifying the extracted points in each region, classifier 642 will not allow the number of extracted points to be classified into each region to exceed a specified number. Specifically, when classifying a predetermined point into a predetermined region corresponding to the predetermined point, if classifier 642 determines that the number of points contained in the predetermined region has reached the specified number, then the predetermined point will not be classified into the predetermined region.

[0078] In classifying the extracted points of each region, classifier 642 ends the classification of the extracted points for each region when it determines that the total number of points classified into each region has reached a predetermined number. Specifically, classifier 642 ends the classification of the extracted points for 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 points in each region. For example, the predetermined number is 201 (specifically, for example, a predetermined number of 67 × 3 = 201), which is a value set by parameter adjustment. In other words, classifier 642 ends the classification of the extracted points for each region when it determines that the number of points classified into each region has reached the predetermined number. Specifically, classifier 642 ends the classification of the extracted points for 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 the predetermined number.

[0079] In classifying the extracted points in each region, if the classifier 642 determines that the total number (cumulative number) of the extracted points up to the previous point is above a threshold, then even if the number of points contained in the corresponding region of a subsequent extracted point has reached a predetermined number, the subsequent extracted points will still be classified into the region corresponding to that point. The threshold is, for example, ten times a predetermined number (specifically, for example, a predetermined number of 201 × 10 = 2010), which is a value set by parameter adjustment.

[0080] In detail, if the regions corresponding to the extraction points are concentrated in one area, and the total number of points classified into each region does not reach the predetermined number, the classification of extraction points into each region will not end. Therefore, even if a critical number of points are extracted, if the total number of points classified into each region does not reach the predetermined number, subsequent extraction points will still be classified into the region corresponding to that point, even if the number of points in the region corresponding to the extracted point has reached the prescribed number. In this way, the classification of extraction points into each region can end when the total number of points classified into each region reaches the predetermined number.

[0081] Comparator 643 compares the points classified into each region with the points in the 3D point group data contained in the map information of the map relating to the moving body 1's movement, thereby estimating the moving body 1's self-position. Specifically, comparator 643 compares all the points classified into each region with the points contained in the 3D point group data of the 3D map (map information), thereby estimating the moving body 1's self-position.

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

[0083] Next, a flowchart is used to illustrate the self-position estimation process performed by the state estimator 64 (an example of a control method for the moving body 100). Figure 9 shows a flowchart of the self-position estimation process performed by the state estimator 64.

[0084] In step S11, the acquirer 641 performs an acquisition process. Specifically, the acquirer 641 acquires three-dimensional point group data representing the position information of the structures surrounding the moving body 1.

[0085] In step S12, classifier 642 performs classification processing. Specifically, classifier 642 successively extracts at least one point from the complex points contained in the three-dimensional point group data of the structure, and classifies the extracted point into the corresponding region in the three directions of the coordinate system of the moving body 1, until the number of points contained in each region reaches a specified number.

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

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

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

[0089] In step S23, the acquirer 641 removes nearby point data from the total point data. Specifically, the acquirer 641 searches for a second point among all points in the total point data whose distance from the moving body 100 is less than a second set value, and removes the second point from all points. The second set value is less than the first set value, and is, for example, a value set by parameter adjustment.

[0090] In step S24, the acquirer 641 removes the point group data of the moving body 1 from the total point group data. Specifically, the acquirer 641 searches for the third point obtained by measuring the moving body 1 itself from all the points contained in the total point group data, and removes the third point from all the points.

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

[0092] The preset 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, for example, an error of ±10cm. Points at the first predetermined value are points included in the three-dimensional point data of the floor. The second predetermined value is the ceiling height, including, for example, an error of ±10cm. Points at the second predetermined value are points included in the three-dimensional point data of the ceiling. The third predetermined range is the range from the height of the obstacle to the height of the ceiling. Points at the third predetermined range are points included in the three-dimensional point data of the walls.

[0093] In step S26, the acquirer 641 acquires the three-dimensional point group data of the structure. Specifically, the acquirer 641 acquires the three-dimensional point group data of the walls, floor, or ceiling. Then, return to the flowchart in Figure 9.

[0094] Figure 11 is a flowchart of the sub-procedure of the classification process (step S12). First, in step S31, the classifier 642 randomly selects one point from the complex points contained in the three-dimensional point group data of the structure obtained by the acquirer 641.

[0095] In step S32, classifier 642 explores the region corresponding to the extraction point among the three directions of the coordinate system of the moving body 1 (hereinafter also referred to as the corresponding region). Specifically, as described above, the classification method of extraction points of each region by classifier 642 explores the first region, the second region, or the third region as the corresponding region of the extraction point.

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

[0097] When classifier 642 determines that the number of points contained in the corresponding region has not reached the specified number, in step S36, classifier 642 classifies the extracted points into the corresponding region. When classifier 642 determines that the number of points contained in the corresponding region has reached the specified number, in step S34, classifier 642 determines whether the total number of extracted points other than the extracted point extracted this time up to the previous step is above the critical value.

[0098] If classifier 642 determines that the total number of extraction points up to the previous step is above the critical value, in step S36, classifier 642 classifies the extraction points into the corresponding region. If classifier 642 determines that the total number of extraction points up to the previous step is not above the critical value, in step S35, classifier 642 does not classify the extraction points into the corresponding region but removes the extraction points.

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

[0100] When classifier 642 determines that the total number of points classified into each region has reached the predetermined number, in step S38, classifier 642 ends the classification of the extracted points in each region. Then, return to the flowchart in Figure 9.

[0101] When classifier 642 determines that the total number of points classified into each region has not reached the predetermined number, in step S31, classifier 642 randomly selects a point from the complex points contained in the three-dimensional point group data of the structure. At this time, classifier 642 removes the previously selected point from the complex points and selects a new point. Then, classifier 642 repeats steps S32 to S37.

[0102] Figure 12 is a schematic diagram illustrating the classification process performed by classifier 642. Figure 12 corresponds to Figure 7. Figure 12 is a schematic diagram showing the relationship between the first region Z1 and the second region Z2 of the moving body 1 and the points P contained in the three-dimensional point group data of the wall W under top-down view, which simplifies the three-dimensional representation to two-dimensional representation. In Figure 12, the same symbols as in Figure 7 have the same structure, so their description is omitted. In Figure 12, the obstacle 30 exists in the left-right direction (Y direction) of the moving body 1, resulting in very few three-dimensional point group data of the wall W in the left-right direction of the moving body 1 obtained by the acquisition device 641. That is, the number of points P contained in the three-dimensional point group data of the wall W in the left-right direction of the moving body 1 obtained by the acquisition device 641 is much smaller than the number of points P contained in the three-dimensional point group data of the wall W in the front-back direction (X direction) of the moving body 1 obtained by the acquisition device 641.

[0103] The classifier 642 classifies points P contained in the 3D point group data of the walls W located in the front-back direction of the moving body 1 into the first region Z1, and classifies points P contained in the 3D point group data of the walls W located in the left-right direction of the moving 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 surrounded by dashed circles. For ease of understanding, the predetermined number for each region is set to three. Thus, even if the number of points P in the Y direction obtained by the acquirer 641 is much smaller than the number of points P in the X direction obtained by the acquirer 641, the classifier 642 can still 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) obtains three-dimensional point group data of the structures surrounding the mobile body 1, and successively extracts at least one point from the plurality of points contained in the three-dimensional point group data of the structures, and classifies the extracted point into the corresponding region in the three directions of the mobile body 1 until the number of points contained in each region reaches a predetermined number, and compares the points classified into each region with the points in the three-dimensional point group data contained in the map information to estimate the self-position of the mobile body 1.

[0105] Therefore, even if the point data of structures in a specific direction of the mobile body 1 becomes extremely scarce due to the position of obstacles relative to the mobile body 100, the control device 6 can still compare the points classified into each region with the points in the three-dimensional point data contained in the map information after classifying the points into each region of the mobile body 1 until the number of points contained in each of the three regions of the mobile body 1 reaches a predetermined number. Thus, in the scanning and matching of the point data classified into each region with the three-dimensional point data contained in the map information, the balance between the regions in the three directions of the mobile body 1 can be improved, and the reduction in the accuracy of the mobile body 1's self-position estimation can be mitigated through simple comparison operations without increasing the computational load.

[0106] Furthermore, since the control device 6 extracts one point from the multiple points contained in the three-dimensional point group data of the structure one by one, and classifies the extracted points into the corresponding regions in the three directions until the number of points contained in each region reaches the specified number, the computational load can be reduced compared to classifying all points contained in the three-dimensional point group data of the structure into each region.

[0107] Furthermore, since the control device 6 selects and obtains the three-dimensional point group data of the structure from the three-dimensional point group data of the objects surrounding the moving body 1, the three-dimensional point group data of the structure can be easily obtained.

[0108] Furthermore, since the control device 6 does not classify the extracted points into more than the prescribed number for each region, the deviation in the number of points classified into each region can be reduced. Therefore, in the scanning and matching of the point group data classified into each region with the three-dimensional point group data contained in the map information, the balance between regions in the three directions of the moving body 1 can be improved, and the decrease in the accuracy of the moving body 1's self-position estimation can be reduced.

[0109] Furthermore, since the control device 6 ends the classification of extracted points in each region when it determines that the total number of points classified into each region has reached a predetermined number, the deviation in the number of points classified into each region can be reduced. Therefore, in the scanning and matching of the point group data classified into each region with the three-dimensional point group data contained in the map information, the balance between regions in the three directions of the moving body 1 can be improved, and the decrease in the accuracy of the moving body 1's self-position estimation can be reduced.

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

[0111] Furthermore, since the control device 6 classifies the subsequently extracted points into the corresponding regions even if the number of points in the regions corresponding to the extracted points has reached the predetermined number when it determines that the total number of points extracted up to the previous point is above the critical value, the classification of the extracted points in each region can be terminated when the total number of points classified into each region reaches the predetermined number. Therefore, it can prevent the computational load from increasing.

[0112] Furthermore, since the sensor 3 is located at the lower part of the mobile body 1, there may be situations where the sensor 3 cannot detect structures due to obstacles located near the mobile body 100. However, since the mobile body 100 has the aforementioned control device 6, even if the position of the obstacle relative to the mobile body 100 results in very little point group data of structures in a specific direction of the mobile body 1 obtained by the acquisition device 641, the decrease in the accuracy of the self-position estimation of the mobile body 1 can be mitigated. Therefore, it is preferable that the sensor 3 is located at the lower part of the mobile body 1 in the mobile body 1 of the present invention.

[0113] Furthermore, the mobile body 1 is a robot that includes a robotic arm 12 and is movable. When the mobile body 1 moves within facilities such as shops, hospitals, or care centers, even if the point group data of the structures in a specific direction of the mobile body 1 obtained by the acquisition device 641 within the facility becomes very scarce, the decrease in the accuracy of the mobile body 1's self-position estimation can be mitigated.

[0114] The control device 6 can also control the robotic arm 12 while performing autonomous movement. Figure 13 is a functional block diagram showing the configuration of the control system of the processor 61 in the modified example. The processor 61 can also function as an arm controller 611 to control the robotic arm 12.

[0115] The arm controller 611 causes the robotic arm 12 to move. For example, the arm controller 611 causes the robotic arm 12 to deform into a target shape. The arm controller 611 can also keep the robotic arm 12 in the target shape. The arm controller 611 can also cause the robotic arm 12 to move by continuously changing the shape of the robotic arm 12.

[0116] The arm controller 611 generates command values ​​corresponding to the target shape of the robotic arm 12. Based on these command values, the arm controller 611 calculates individual command operations for a plurality of motors 12a. For example, the operation operation is the motor's rotational speed or torque.

[0117] The arm controller 611 can also keep the robotic arm 12 in a certain shape when the moving body 100 is walking, and make the robotic arm 12 move when it is to perform a task.

[0118] For example, when the moving body 100 is moving, the arm controller 611 keeps the robotic arm 12 in the walking shape. In other words, the arm controller 611 keeps the shape of the robotic arm 12 fixed and prevents the robotic arm 12 from moving when the moving body 100 is moving.

[0119] Figure 14 is a side view of the moving body 1 of the robotic arm 12 in a walking position. Figure 15 is a top view of the moving body 1 of the robotic arm 12 in a walking position.

[0120] For example, the walking-shaped robotic arm 12 is positioned at a higher position. For example, the intermediate joint between the shoulder and wrist joints of the walking-shaped robotic arm 12, such as the fourth joint J4, is bent; the portion between the base 11 and the intermediate joint extends backward and downward from the base 11; and the portion between the intermediate joint and the wrist joint extends forward from the intermediate joint. That is, the walking-shaped robotic arm 12 has a shape where the intermediate joint is pulled backward and bent. This allows the portion of the robotic arm 12 relative to the front end of the intermediate joint to be positioned higher. Furthermore, the front end of the robotic arm 12 can be positioned further rearward.

[0121] The walking-shaped robotic arm 12 is positioned above the first sensor 3A of the moving 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 robotic arm 12 is positioned above the first sensor 3A, it may obstruct a portion of the detection range of the first sensor 3A. Among the detection results of the first sensor 3A, the detection results corresponding to the robotic arm 12 are treated as invalid detection results. The higher the position of the robotic arm 12, the further away the robotic arm 12 is from the first sensor 3A. And the further away the robotic arm 12 is from the first sensor 3A, the smaller the area of ​​the first sensor 3A's detection range that is obstructed by the robotic arm 12 tends to be. Therefore, with the above-described walking shape, the first sensor 3A can ensure a larger detection range.

[0122] Furthermore, the walking-shaped robotic arm 12 extends forward from the base 11 by a smaller amount. By extending forward by a smaller amount, the detection range of the larger first sensor 3A can be ensured, which is tilted forward and upward from the first sensor 3A.

[0123] The width of the overall shape of the robotic arm 12 in its walking configuration, viewed from above, becomes smaller. For example, in its walking configuration, the second link L2 is located on the outermost side in the width direction. The links other than the second link L2 among the plurality of links L are positioned further inward in the width direction compared to the second link L2. By reducing the width of the overall shape of the robotic arm 12 in its walking configuration, viewed from above, the possibility of interference between the robotic arm 12 and other objects in the width direction during walking can be reduced. Furthermore, the robotic arm 12 in its walking configuration can also rotate the first link L1 forward about the rotation axis of the first joint J1, positioning the first link L1 and the second link L2 further forward of the rotation axis of the first joint J1 in the forward-backward direction. This further reduces the width of the second link L2 of the two robotic arms, i.e., the width of the overall shape of the robotic arm 12 in its overall shape, viewed from above.

[0124] The overall shape of the robotic arm 12, viewed from above, converges inward in the longitudinal direction towards the trolley 10. This reduces the possibility of interference between the robotic arm 12 and other objects in the longitudinal direction during movement.

[0125] Furthermore, the individual shapes of the walking patterns of the two robotic arms 12 may not be exactly the same. That is, the shapes of the two robotic arms 12 may also be slightly different. For example, the height of the front end of one robotic arm 12 and the front end of the other robotic arm 12 may be different; the rotation angle of the seventh joint J7 of one robotic arm 12 and the seventh joint J7 of the other robotic arm 12 may also be different.

[0126] For example, the arm controller 611 causes the robotic arm 12 to move when it is to perform a task. In other words, the arm controller 611 allows the robotic arm 12 to move freely when it is to perform a task. For example, the arm controller 611 causes the robotic arm 12 to move after the moving body 1 reaches the destination, so that the robotic arm 12 can perform the task.

[0127] Other Implementation Modes As described above, the embodiments of the technology disclosed in this invention have been illustrated. However, this invention can also be applied to embodiments that have been adapted, modified, substituted, added to, or omitted, and is not limited to the aforementioned embodiments. Furthermore, the constituent elements described in the aforementioned embodiments can be combined to form new embodiments. Also, for the purpose of illustrating the aforementioned technology, the constituent elements described in the drawings and specification include not only those necessary to solve the problem, but also those not necessary to solve the problem. Therefore, even if such non-necessary constituent elements are described in the drawings and specification, they should not be automatically considered as necessary constituent elements.

[0128] For example, the mobile body 1 is not limited to a robot but can also be a mobile device such as a drone, ship, or vehicle. The location where the mobile body 1 moves is not limited to a passageway but can also be a general road or waterway. The mobile body 1 may also not include the robotic arm 12. The base 11 can also rotate relative to the trolley 10. The wheel 13 is not limited to an omnidirectional wheel. When the wheel 13 is an omnidirectional wheel, it can also be an omni-wheel.

[0129] Sensor 3 is not limited to LiDAR. Sensor 3 can also be a two-dimensional or three-dimensional camera. Sensor 3 can also be a three-dimensional scanner. Sensor 3 can also be configured outside the trolley 10 of the moving body 1. The number of sensors 3 is not limited to three. The number of sensors 3 can also be one, two, or more than four. The scanning range of the measurement light of the aforementioned first sensor 3A, second sensor 3B, and third sensor 3C is only illustrative. For example, the first sensor 3A can also scan the measurement light over a region that includes at least the front area of ​​the moving body 1. The first sensor 3A can also scan the measurement light over a range from the left rear to the right rear of the moving body 1, including the front of the moving body 1. Furthermore, the second sensor 3B and the third sensor 3C can each scan 360 degrees in the horizontal direction with measurement light. The second sensor 3B can also scan a predetermined range (not limited to 270 degrees) including the left rear area of ​​the moving body 1 with measurement light. For example, the second sensor 3B can also measure a 360-degree light scan in a horizontal direction. The third sensor 3C can also measure a light scan over a predetermined range (not limited to 270 degrees) including the area to the right rear of the moving body 1. For example, the third sensor 3C can also measure a 360-degree light scan in a horizontal direction.

[0130] In the implementation, classifier 642 classifies the points contained in the three-dimensional point group data of the floor or ceiling obtained by acquirer 641 to the third region of the moving body 1, but it can also classify the points contained in the three-dimensional point group data of the floor and ceiling to the third region.

[0131] In the implementation, the classifier 642, when classifying the extracted points in each region, determines that the total number of extracted points up to the previous point is above the threshold. Even if the number of points in the corresponding region of the subsequent extracted point has reached the specified number, the subsequent extracted points will still be classified into the region corresponding to that point. However, if the number of points in the corresponding region of the subsequent extracted point has reached the specified number, the subsequent extracted points may not be classified into the region corresponding to that point.

[0132] The flowchart is for illustrative purposes only. The steps in the flowchart can be adapted, modified, replaced, added, or omitted. Furthermore, the order of the steps in the flowchart can be changed, and sequential processes can be changed to parallel processes.

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

[0134] The functions of the elements disclosed in this specification can be implemented using one or more circuits or processing circuits including general-purpose processors, special-purpose processors, integrated circuits, application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), and / or conventional circuits. The functions of the elements disclosed in this specification can also 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 can also be programmed using one or more programs stored together or individually in one or more memories, or configured by other methods to perform the disclosed functions. A processor, because it includes transistors and other circuits, is also considered a processing circuit or circuit. A processor can also be a processor that executes programs stored in memory. In this invention, circuits, units, or means are hardware that individually or in combination executes the listed functions, or hardware that individually or in combination programs the listed functions into executable functions. If the hardware is the hardware disclosed in this specification and is programmed or configured to perform the listed functions, then it may be any hardware.

[0135] Computer programs containing computer commands are stored in memory. Computer commands provide logic and procedures that enable hardware to execute the methods disclosed in this specification. Hardware may include, for example, processing circuitry or circuitry. Computer programs can be installed in known formats on computer-readable recording media, computer programs, memory devices, recording media such as CD-ROMs or DVDs, and / or the memory of FPGAs or ASICs.

[0136] [State / Appearance] The above implementation type is a specific example of the following type.

[0137] (State 1) A 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 estimating the self-position of the mobile body 1 based on the detection signals of the sensor 3 and for causing the mobile body 1 to perform autonomous movement. The control device 6 obtains three-dimensional point group data representing the position information of structures around the mobile body 1 from the detection signals of the sensor 3; sequentially extracts at least one point from the plurality of points contained in the three-dimensional point group data of the structures, and classifies the extracted point into corresponding regions in three directions of the coordinate system of the mobile body 1 until the number of points contained in each region reaches a predetermined number; and compares the points classified into each region with the points in the three-dimensional point group data contained in the map information of a map relating to the environment in which the mobile body 1 moves, thereby estimating the self-position of the mobile body 1.

[0138] Based on this configuration, even if the point group data of the structure in a specific direction of the moving body 1 becomes very small due to the position of the obstacle relative to the moving body 100, the reduction in the accuracy of the self-position estimation of the moving body 1 can be reduced by simple comparison operation without increasing the computational load.

[0139] (State 2) In the moving body 100 as described in state sample 1, The control device 6 selects and obtains the three-dimensional point group data of the structure from the three-dimensional point group data representing the three-dimensional position information of the objects surrounding the moving body 1 during the acquisition of the three-dimensional point group data of the structure.

[0140] Based on this structure, three-dimensional point group data of the structure can be easily obtained.

[0141] (State 3) In the moving body 100 as described in state sample 1 or state sample 2, The control device 6 will prevent the number of extracted points classified into each region from exceeding the prescribed number during the classification of extracted points in each region.

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

[0143] (Pattern 4) In any of the states 1 to 3, the moving body 100, When the control device 6 determines that the total number of points classified into each region has reached a predetermined number, it ends the classification of the extracted points in each region.

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

[0145] (Style 5) In any of the states 1 to 4, the moving body 100, The predetermined quantity is the total number of the prescribed quantities for each region.

[0146] Based on this structure, since the predetermined quantity is the total number of the prescribed quantities for each region, the classification of the extraction points for each region can be terminated when the number of points classified into each region has reached the prescribed quantity.

[0147] (Style 6) In any of the states 1 to 5, the moving body 100, In classifying the extracted points in each region, the control device 6, when determining that the total number of extracted points up to the previous point is above the critical value, will classify the subsequently extracted points into the region corresponding to that point, even if the number of points in the region corresponding to the subsequently extracted point has reached the prescribed number.

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

[0149] (Style 7) In any of the states 1 to 6, the moving body 100, The sensor 3 is located on the lower part of the moving body 1.

[0150] Accordingly, the mobile body 100 of the present invention preferably has the sensor 3 disposed at the lower part of the mobile body 1.

[0151] (Style 8) In any of the states 1 to 7, the moving body 100, The mobile body 1 is a robot that includes a robotic arm 12 and is movable.

[0152] Based on this configuration, when the mobile body 1 moves within a facility such as a shop, hospital, or care center, even if the point group data of the structures in a specific direction of the mobile body 1 becomes extremely scarce within the facility, the decrease in the accuracy of the mobile body 1's self-position estimation can be mitigated.

[0153] (Style 9) A control device 6 for a mobile body estimates the self-position of the mobile body 1 while enabling the mobile body 1 to perform autonomous movement. The control device 6 includes: a device 641 for acquiring three-dimensional point group data representing the position information of structures around the mobile body 1; a classifier 642 for successively extracting at least one point from a plurality of points contained in the three-dimensional point group data of the structures and classifying the extracted point into a corresponding region in the three-directional regions of the coordinate system of the mobile body 1 until the number of points contained in each region reaches a predetermined number; and a comparator 643 for estimating the self-position of the mobile body 1 by comparing the points classified into each region with the points in the three-dimensional point group data contained in the map information of the map of the environment in which the mobile body 1 moves.

[0154] Based on this configuration, even if the point group data of the structure in a specific direction of the moving body 1 becomes very small due to the position of the obstacle relative to the moving body 100, the reduction in the accuracy of the self-position estimation of the moving body 1 can be reduced by simple comparison operation without increasing the computational load.

[0155] (Style 10) A method for controlling a mobile body involves estimating the self-position of the mobile body 1 while enabling the mobile body 1 to perform autonomous movement. The control method includes the following steps: obtaining three-dimensional point group data representing the position information of structures surrounding the mobile body 1; sequentially extracting at least one point from the plurality of points contained in the three-dimensional point group data of the structures, and classifying the extracted point into corresponding regions in three directions of the coordinate system of the mobile body 1, until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with the points in the three-dimensional point group data contained in the map information of a map relating to the environment in which the mobile body 1 moves, thereby estimating the self-position of the mobile body 1.

[0156] Based on this configuration, even if the point group data of the structure in a specific direction of the moving body 1 becomes very small due to the position of the obstacle relative to the moving body 100, the reduction in the accuracy of the self-position estimation of the moving body 1 can be reduced by simple comparison operation without increasing the computational load.

[0157] (Style 11) A control program for a mobile body is used to estimate the self-position of the mobile body 1 and enable the mobile body 1 to perform autonomous movement. The control program enables a computer to perform the following functions: acquiring three-dimensional point group data representing the position information of the structures surrounding the mobile body 1; successively extracting at least one point from the plurality of points contained in the three-dimensional point group data of the structures and classifying the extracted point into the corresponding region in the three-directional regions of the coordinate system of the mobile body 1 until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with the points in the three-dimensional point group data contained in the map information of the map of the environment in which the mobile body 1 moves, thereby estimating the self-position of the mobile body 1.

[0158] Based on this configuration, even if the point group data of the structure in a specific direction of the moving body 1 becomes very small due to the position of the obstacle relative to the moving body 100, the reduction in the accuracy of the self-position estimation of the moving body 1 can be reduced by simple comparison operation without increasing the computational load.

[0159] 1: The moving body itself 3: Sensor 3A: First Sensor 3B: Second sensor 3C: Third Sensor 6: Control device 10: trolley 11: Base 12: Robotic Arm 12a: Motor 12b: Encoder 13: Wheels 13a: Motor 13b: Encoder 14: Hands 30: Obstacles 61: Processor 62: Storage 63: Memory 64: State Estimator 65: Map Generator 66: Path Generator 67: Track Generator 68: Motion Controller 69: Operation Calculator 100: Moving body 611: Arm Controller 641: Acquirer 642: Classifier 643: Comparator F: Range J1: First joint J2: Second joint J3: Third joint J4: Fourth joint J5: Fifth Joint J6: Sixth Joint J7: Seventh Joint L1: First Link L2: Second Link L3: Third Link L4: Fourth Link L5: Fifth Link L6: Sixth Link L7: Seventh Link N: Normal direction P: Point (Extraction Point) S1,S2,S3,S4,S5,S11,S12,S13,S21,S22,S23,S24,S25,S26,S31,S32,S33,S34,S35,S36,S37,S38: Steps W: Wall Z1: First Area Z2: Second Region Z11: Front Area Z12: Rear area Z21: Left Region Z22: Right Region

Claims

1. A mobile body comprising: a mobile body body; a sensor for detecting objects around the mobile body body; and a control device for estimating the self-position of the mobile body body based on detection signals from the sensor, and for causing the mobile body body to perform autonomous movement; the control device acquiring three-dimensional point group data representing position information of structures around the mobile body body from the detection signals from the sensor; sequentially extracting at least one point from a plurality of points contained in the three-dimensional point group data of the structures, and classifying the extracted point into corresponding regions in three directions of the coordinate system of the mobile body body, until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with points in the three-dimensional point group data contained in map information of a map relating to the environment in which the mobile body body moves, thereby estimating the self-position of the mobile body body.

2. The moving body as described in claim 1, wherein, The control device selects and obtains the three-dimensional point group data of the structure from the three-dimensional point group data representing the three-dimensional position information of the objects surrounding the moving body during the acquisition of the three-dimensional point group data of the structure.

3. The moving body as described in claim 1, wherein, The control device ensures that the number of extracted points assigned to each region does not exceed a specified number during the classification of extracted points in each region.

4. The moving body as described in claim 3, wherein, The control device ends the classification of the extracted points in each region when it determines that the total number of points classified into each region has reached a predetermined number.

5. The moving body as described in claim 4, wherein, The predetermined quantity is the total number of the prescribed quantities for each region.

6. The moving body as described in claim 4, wherein, In classifying the extracted points in each region, the control device, when determining that the total number of extracted points up to the previous point is above a critical value, will classify the subsequently extracted points into the corresponding region even if the number of points in the region corresponding to the subsequently extracted points has reached a predetermined number.

7. The moving body as described in claim 1, wherein, The sensor is located on the lower part of the moving body.

8. The moving body as described in claim 1, wherein, The mobile body is a robot that includes a robotic arm and is capable of movement.

9. A control device for a mobile body, which estimates the self-position of the mobile body and enables the mobile body to perform autonomous movement; the control device comprises: a acquirer that acquires three-dimensional point group data representing the position information of structures surrounding the mobile body; a classifier that sequentially extracts at least one point from a plurality of points contained in the three-dimensional point group data of the structures and classifies the extracted point into a corresponding region among three directions in the coordinate system of the mobile body, until the number of points contained in each region reaches a predetermined number; and a comparator that compares the points classified into each region with points in the three-dimensional point group data contained in map information of a map relating to the environment in which the mobile body moves, thereby estimating the self-position of the mobile body.

10. A method for controlling a mobile body, wherein the mobile body is autonomously moved while its self-position is estimated; the control method includes the following steps: obtaining three-dimensional point group data representing the position information of structures surrounding the mobile body; sequentially extracting at least one point from a plurality of points contained in the three-dimensional point group data of the structures, and classifying the extracted point into corresponding regions in three directions of the coordinate system of the mobile body, until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with points in the three-dimensional point group data contained in map information of a map relating to the environment in which the mobile body is moving, thereby estimating the self-position of the mobile body.

11. A control program for a mobile body, used to estimate the self-position of the mobile body and enable the mobile body to perform autonomous movement; the control program enables a computer to perform the following functions: acquiring three-dimensional point group data representing the position information of structures surrounding the mobile body; successively extracting at least one point from a plurality of points contained in the three-dimensional point group data of the structures, and classifying the extracted point into corresponding regions in three directions of the coordinate system of the mobile body until the number of points contained in each region reaches a predetermined number; and comparing the points classified into each region with the points in the three-dimensional point group data contained in the map information of a map relating to the environment in which the mobile body is moving, thereby estimating the self-position of the mobile body.

Images