Method for controlling a mobile body, mobile body, and computer-readable storage medium

Abstract

A mobile body control method, a mobile body, and a computer-readable storage medium cause a mobile body to appropriately move to a destination position. The mobile body control method is a control method of a mobile body that moves automatically, and includes the steps of causing a sensor provided to the mobile body to detect a target object multiple times, acquiring a detection result of each detection of the sensor as a point group; estimating a position and an attitude of the target object for each detection of the sensor based on the point group; calculating, for each detection of the sensor, a reliability in terms of accuracy of an estimation result for the position and the attitude of the target object; selecting, from the estimation result of each detection of the sensor, an estimation result for setting an approach path to a target position that becomes a prescribed position and attitude with respect to the target object, based on the reliability of each detection of the sensor; setting the approach path based on the selected estimation result; and causing the mobile body to move along the approach path.

Description

Technical Field

[0001] This invention relates to a method for controlling a mobile body, a mobile body, and a computer-readable storage medium. Background Technology

[0002] It is known that there are mobile bodies equipped with sensors that detect their surroundings and move automatically. Examples of such mobile bodies include automated forklifts that transport pallets loaded with goods. Patent Document 1 describes a method for picking up goods from a pallet by determining the center position and orientation of the front surface of the pallet based on distance data measured by a range sensor.

[0003] Prior art literature

[0004] Patent documents

[0005] Patent Document 1: Japanese Patent Application Publication No. 2017-178567 Summary of the Invention

[0006] The problem that the invention aims to solve

[0007] For such a moving object, the aim is to move it appropriately to the desired location.

[0008] The present invention is a technology for solving the above-mentioned problems, and its purpose is to provide a control method for a mobile body that enables a mobile body to be appropriately moved to a target position, a mobile body, and a computer-readable storage medium.

[0009] Solution for solving the problem

[0010] To address the aforementioned issues and achieve the objectives, the present invention provides a method for controlling an automatically moving body, comprising the following steps: having a sensor disposed on the moving body repeatedly detect a target object, obtaining the detection results of each sensor detection as a point group; estimating the position and orientation of the target object based on the point group for each sensor detection; calculating the reliability of the estimation results for the position and orientation of the target object in terms of accuracy for each sensor detection; selecting, from the estimation results of each sensor detection, an estimated result for setting an approach path to a target position relative to the target object that achieves a predetermined position and orientation, based on the reliability of each sensor detection; setting the approach path based on the selected estimated result; and moving the moving body along the approach path.

[0011] To address the aforementioned issues and achieve the objective, the mobile body of the present invention is an automatically moving mobile body, comprising: a detection control unit that causes a sensor provided on the mobile body to repeatedly detect a target object and obtains the detection results of each detection by the sensor as a point group; an approach path information acquisition unit that acquires information about an approach path set based on the point group up to a target position with a predetermined position and attitude relative to the target object; and a movement control unit that causes the mobile body to move along the approach path, the approach path being set as follows: based on the point group, the position and attitude of the target object are estimated for each detection by the sensor; for each detection by the sensor, a reliability in terms of the accuracy of the estimated position and attitude of the target object is calculated; based on the reliability of each detection by the sensor, an estimated result for setting the approach path is selected from the estimated results of each detection by the sensor; and the approach path is set based on the selected estimated result.

[0012] To address the aforementioned issues and achieve the objectives, the storage medium of the present invention is a computer-readable storage medium storing a program for controlling a moving body that enables a computer to execute automatic movement. The program includes the following steps: having a sensor disposed on the moving body repeatedly detect a target object, obtaining the detection results of each sensor detection as a point group; estimating the position and orientation of the target object based on the point group for each sensor detection; calculating the reliability of the estimation results for the position and orientation of the target object in terms of accuracy for each sensor detection; selecting, from the estimation results of each sensor detection, an estimated result for setting an approach path to a target position relative to the target object that achieves a predetermined position and orientation, based on the reliability of each sensor detection; setting the approach path based on the selected estimation result; and moving the moving body along the approach path.

[0013] Invention Effects

[0014] According to the present invention, a moving body can be appropriately moved to a target position. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the motion control system according to the first embodiment.

[0016] Figure 2 This is a schematic diagram of the structure of the moving body.

[0017] Figure 3 It is a schematic block diagram of the management system.

[0018] Figure 4 It is a schematic block diagram of an information processing device.

[0019] Figure 5 This is a schematic block diagram of the control device for the moving body.

[0020] Figure 6 This is a schematic diagram illustrating the detection of the target object.

[0021] Figure 7 This is a schematic diagram used to illustrate the acquisition of approximate line candidates.

[0022] Figure 8 This is a schematic diagram illustrating a specific example of a candidate approximation line.

[0023] Figure 9 This is a chart showing an example of a fraction.

[0024] Figure 10 This is a schematic diagram illustrating an example of the process of determining the two endpoints of a target object.

[0025] Figure 11 This is a schematic diagram used to illustrate the calculation method for reliability.

[0026] Figure 12 This is a schematic diagram illustrating an example of movement along the second path.

[0027] Figure 13 This is a flowchart illustrating the processing flow of the second path setting.

[0028] Figure 14 This is a schematic diagram illustrating the method for calculating the ideal number of measurement points in the second embodiment.

[0029] Explanation of reference numerals in the attached figures:

[0030] 10...moving body; 26...sensor; 70...first path information acquisition unit; 72...movement control unit; 74...detection control unit; 76...target information acquisition unit; 78...reliability information acquisition unit; 80...selection information acquisition unit; 82...second path information acquisition unit; M...measuring point; M0...point group; P...target object; R2...second path. Detailed Implementation

[0031] The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be noted that the present invention is not limited to these embodiments. In addition, when there are multiple embodiments, the invention also includes solutions that combine the various embodiments.

[0032] (First Implementation)

[0033] (Overall structure of the motion control system)

[0034] Figure 1 This is a schematic diagram of the motion control system according to the first embodiment. (As shown) Figure 1 As shown, the movement control system 1 of the first embodiment includes a moving body 10, a management system 12, and an information processing device 14. The movement control system 1 is a system for controlling the movement of the moving body 10 belonging to the equipment W. The equipment W is, for example, a warehouse or other equipment for logistics management. In the movement control system 1, the moving body 10 picks up and transports a target object P disposed in an area AR of the equipment W. The area AR is, for example, the ground of the equipment W, and is an area where the target object P or the moving body 10 is located and moved. In this embodiment, the target object P is a transport object loaded with goods on a pallet. The target object P has a plurality of posts PA formed on its front surface Pa, and an opening PB formed between the posts PA. The front surface Pa refers to the surface on the side approaching the moving body 10. The moving body 10 holds the target object P by inserting the fork 24 (described later) into the opening PB. However, the target object P is not limited to the form of loading goods on a pallet, but can be any form, for example, it can be goods without a pallet. Hereinafter, one direction along the area AR will be defined as direction X, and the direction along the area AR and intersecting with direction X will be defined as direction Y. In this embodiment, direction Y is the direction orthogonal to direction X. Directions X and Y can also be referred to as horizontal directions. Furthermore, the direction orthogonal to directions X and Y, i.e., the vertical direction, is designated as direction Z.

[0035] Within device W, area AR contains multiple setting areas AR0. Setting area AR0 is the area where target object P is set. Setting area AR0 is pre-defined as the area where target object P should be set. Setting area AR0 is divided, for example, by white lines, and its position (coordinates), shape, and size are pre-defined. Within setting area AR0, target object P is positioned with its front surface Pa facing the X direction. Figure 1 In the example, the target object P is positioned within the setting area AR such that its axis PX, which is orthogonal to the front surface Pa when viewed from direction Z, is along direction X, meaning that the orientation of the target object P is not offset relative to the setting area AR0. However, the target object P is not limited to its axis PX being along direction X; it can also be positioned such that its axis PX is tilted relative to direction X, meaning that its orientation is offset relative to the setting area AR. For example, the target object P is preferably positioned in the setting area AR0 such that the tilt angle between its axis PX and direction X is 45 degrees or less.

[0036] It should be noted that in this embodiment, the setting area AR0 is located in area AR, which serves as the floor of the device W, but it is not limited to this. For example, it can also be located in the cargo box of the vehicle used to move the target object P into the device W. Furthermore, in this embodiment, the setting area AR0 is divided for each target object P, and one target object P is configured in the setting area AR0, but it is not limited to this. For example, the setting area AR0 can also be set as free space to accommodate multiple target objects P. Additionally, in... Figure 1 In the example, the region AR0 is set as a rectangle, but its shape and size can be arbitrary. Furthermore, the number of regions AR0 set within region AR can also be arbitrary.

[0037] The moving body 10 is a device capable of automatic movement. In this embodiment, the moving body 10 is a forklift, more specifically, a so-called AGF (Automated Guided Forklift). Figure 1 As illustrated, the mobile body 10 moves within region AR of the device W. The mobile body 10 moves from a first position A1 to a second position A2 along a first path R1 (wide-area path), and repeatedly detects the target object P using the sensor 26 (described later). The mobile body 10 obtains a point group based on the multiple detection results from the sensor 26, and determines the position and orientation of the target object P based on this point group. After reaching the second position A2, the mobile body 10 moves from the second position A2 to the target position A3 along a second path R2 (approach path) set based on the position and orientation of the target object P, in order to pick up the target object P. It should be noted that the position of the target object P here refers to its coordinates in a two-dimensional coordinate system CO of directions X and Y, and the orientation of the target object P refers to its facing direction (rotation angle) when viewed from a direction orthogonal to directions X and Y. Furthermore, the target position A3 is the position and orientation relative to the target object P that defines a specific position and orientation. In this embodiment, the target position A3 can be considered the position and orientation at which the mobile body 10 can pick up the target object P. For example, target position A3 can be the position and orientation of the moving body 10, which moves in a straight line without lateral movement, thereby enabling the forks 24 of the moving body 10 (described later) to insert into the opening PB of the target object P. In this case, the moving body 10 moves in a straight line from target position A3 to pick up the target object P and transport it to another location. It should be noted that the specific methods for determining the position and orientation of the target object P, the details of movement along the first path R1, and the details of movement along the second path R2 will be described later. Furthermore, Figure 1 The first path R1 and the second path R2 shown are examples.

[0038] (Moving entity)

[0039] Figure 2This is a schematic diagram of the structure of the moving body. For example... Figure 2 As shown, the mobile body 10 includes a body 20, a mast 22, forks 24, sensors 26, and a control device 28. The body 20 includes wheels 20A. The mast 22 is located at one end of the body 20 in the longitudinal direction. The mast 22 extends along a vertical direction (here, direction Z) orthogonal to the longitudinal direction. The forks 24 are mounted on the mast 22 in a manner that allows them to move along direction Z. The forks 24 can also move relative to the mast 22 in the lateral direction of the body 20 (a direction intersecting the vertical and longitudinal directions). The forks 24 have a pair of claws 24A and 24B. Claws 24A and 24B extend from the mast 22 toward the front of the body 20. Claws 24A and 24B are arranged separately from each other in the lateral direction of the mast 22. Hereinafter, in the longitudinal direction, the direction of the side of the mobile body 10 where the forks 24 are provided is referred to as the forward direction, and the direction of the side where the forks 24 are not provided is referred to as the rearward direction.

[0040] Sensor 26 detects at least one of the position and orientation of an object present around the vehicle body 20. Alternatively, sensor 26 can be said to detect the position of the object relative to the moving body 10 and the orientation of the object relative to the moving body 10. In this embodiment, sensor 26 is disposed on the mast 22 and at the four corners of the vehicle body 20, namely the left and right ends on the front and rear sides of the vehicle body 20. However, the placement of sensor 26 is not limited to this; it can be disposed at any location, and the number of sensors disposed therein can also be arbitrary. For example, a safety sensor disposed on the moving body 10 can be used as sensor 26. By repurposing a safety sensor, it is not necessary to reconfigure the sensors.

[0041] Sensor 26 detects the position and orientation of an object by detecting (receiving) reflected light from surrounding objects. More specifically, sensor 26 is a light sensor, specifically, it uses a laser beam as light. Sensor 26 detects the position and orientation of the object by detecting the reflected light from the illuminated laser beam. Sensor 26 illuminates a laser beam while scanning in one direction, and detects the position and orientation of the object based on the reflected light from the illuminated laser beam. That is, sensor 26 can also be described as a so-called 2D-LiDAR (Light Detection and Ranging). In this embodiment, sensor 26 scans the laser beam in a horizontal direction, i.e., a direction orthogonal to direction Z. However, sensor 26 is not limited to the above-described sensor; it can be a sensor that detects objects using any method, such as a so-called 3D-LiDAR that scans in multiple directions, or a camera.

[0042] The control device 28 controls the movement of the moving body 10. The control device 28 will be described later.

[0043] (Management System)

[0044] Figure 3 This is a schematic block diagram of the management system. The management system 12 is a system for managing the logistics within equipment W. In this embodiment, the management system 12 is a WMS (Warehouse Management System), but it is not limited to a WMS; it can be any system, such as a back-end system like other production management systems. The location of the management system 12 is arbitrary; it can be located within equipment W or in a separate location to manage equipment W from that location. The management system 12 is a computer, such as... Figure 3 As shown, it includes a communication unit 30, a storage unit 32, and a control unit 34.

[0045] The control unit 34 is a computing device, i.e., a CPU (Central Processing Unit). The control unit 34 includes a job decision unit 36. The control unit 34 implements the job decision unit 36 ​​and performs its processing by reading and executing a program (software) from the storage unit 32. It should be noted that the control unit 34 can execute processing using a single CPU, or it can have multiple CPUs and execute processing through those multiple CPUs. Alternatively, the job decision unit 36 ​​can also be implemented using hardware circuitry. Furthermore, the program used by the control unit 34 stored in the storage unit 32 can also be stored on a recording medium readable by the management system 12.

[0046] The work decision unit 36 ​​determines the target object P that will be transported. Specifically, based on, for example, an input work plan, the work decision unit 36 ​​determines the work content, which represents information about the target object P that will be transported. The work content can also be described as information that determines the target object P that will be transported. In the example of this embodiment, the work content is determined by which equipment, which target object P, and when it will be transported to where. That is, the work content represents information about the equipment that holds the target object P, the target object P, the destination of the target object P, and the transport period of the target object P. The work decision unit 36 ​​sends the determined work content to the information processing device 14 via the communication unit 30.

[0047] (Information processing device)

[0048] Figure 4 This is a schematic block diagram of an information processing device. The information processing device 14 is installed in the device W and is a device for at least sending and receiving information related to the movement of the mobile body 10, i.e., a so-called ground system. The information processing device 14 is a computer, such as... Figure 4As shown, the system includes a communication unit 40, a storage unit 42, and a control unit 44. The communication unit 40 is a module that communicates with the control unit 44 and with external devices such as the management system 12 and the mobile unit 10; for example, it may include an antenna. In this embodiment, the communication method of the communication unit 40 is wireless communication, but the communication method can be arbitrary. The storage unit 42 is a memory that stores various information such as the calculation content and programs of the control unit 44; for example, it includes at least one of a main storage device such as RAM or ROM, and an external storage device such as an HDD.

[0049] The control unit 44 is a computing device, i.e., a CPU. The control unit 44 includes a job content acquisition unit 50, a mobile body selection unit 52, and a first path acquisition unit 54. The control unit 44 reads and executes a program (software) from the storage unit 42, thereby implementing the job content acquisition unit 50, the mobile body selection unit 52, and the first path acquisition unit 54, and performing their processing. It should be noted that the control unit 44 can execute these processes using a single CPU, or it can have multiple CPUs and execute the processing using those multiple CPUs. Alternatively, at least some of the job content acquisition unit 50, the mobile body selection unit 52, and the first path acquisition unit 54 can be implemented using hardware circuitry. Furthermore, the program used by the control unit 44 stored in the storage unit 42 can also be stored on a recording medium readable by the information processing device 14.

[0050] (Work content acquisition department and mobile body selection department)

[0051] The work content acquisition unit 50 acquires information about the work content determined by the management system 12, namely, information about the target object P that will be transported. Based on the information about the target object P in the work content, the work content acquisition unit 50 determines the installation area AR0 where the target object P is located. For example, the target object P and the installation area AR0 where the target object P is located are associated and stored in the storage unit 42. The work content acquisition unit 50 determines the installation area AR0 by reading this information from the storage unit 42. The mobile body selection unit 52 selects the mobile body 10 that will be the target. For example, the mobile body selection unit 52 selects the mobile body 10 that will be the target from among multiple mobile bodies belonging to the device W. The mobile body selection unit 52 can select the mobile body 10 that will be the target using any method, but for example, it can also select a mobile body 10 suitable for transporting the target object P located in the installation area AR0 determined by the work content acquisition unit 50 as the mobile body 10 that will be the target.

[0052] (First Path Acquisition Section)

[0053] The first path acquisition unit 54 acquires information about the first path R1 up to the setting area AR0 determined by the job content acquisition unit 50. The first path R1 is, for example, preset for each setting area AR0. The first path acquisition unit 54 acquires the first path R1 set for the setting area AR0 determined by the job content acquisition unit 50 from, for example, the storage unit 42. Hereinafter, the first path R1 will be described in detail.

[0054] like Figure 1 As shown, the first path R1 is a track that traverses the setting area AR0 (target P) in the direction Y at a position X closer to the setting area AR0 (target P) towards which the moving body 10 is facing. More specifically, the first path R1 includes a detection track R1a and an approach track R1b connected to the detection track R1a.

[0055] like Figure 1 As shown, the detection track R1a is a track that traverses the setting area AR0 (target object P) along the Y direction at a position X-direction relative to the setting area AR0 (target object P). The detection track R1a is preferably set such that the distance in the X direction to the setting area AR0 is within a predetermined range. This predetermined distance is the distance at which the position and orientation of the target object P within the setting area AR0 can be detected by the sensor 26 of the moving body 10 during its movement along the detection track R1a. More specifically, in this embodiment, the detection track R1a is a track from a first position A1 to a second position A2. The first position A1 is set at a position X-direction relative to the setting area AR0 and also opposite to the Y direction relative to the setting area AR0. The second position A2 is set at a position X-direction relative to the setting area AR0 and also opposite to the Y direction relative to the setting area AR0. In this embodiment, the first position A1 and the second position A2 are set such that the distance in the X direction to the setting area AR0 is within a predetermined range, and the positions (X-coordinates) of the first position A1 and the second position A2 in the X direction are the same. The detection track R1a is set as a straight track along the Y direction from the first position A1 to the second position A2. However, the positions (X coordinates) of the first position A1 and the second position A2 in the X direction may not be the same. In addition, the detection track R1a is not limited to a straight track, but can also be a track that draws an arbitrary trajectory from the first position A1 to the second position A2.

[0056] like Figure 1As shown, the approach track R1b is the track from the second position A2 toward the setting area AR0. More specifically, the approach track R1b is the track from the second position A2 to the setting position A3z. The setting position A3z is the position and attitude relative to the target object P, assuming that the position and attitude of the target object P within the setting area AR0 meet the specified conditions (the target object P is ideally configured without any offset relative to the setting area AR0). That is, the setting position A3z is the position and attitude of the moving body 10 that allows it to pick up the target object P, assuming that the position and attitude of the target object P meet the specified conditions; it can also be described as the target position A3 when the position and attitude of the target object P meet the specified conditions. Figure 1 In the example, the approach track R1b includes a straight track from the second position A2 to an intermediate position ASB0 that is on the opposite side of the second position A2 in the direction Y, and a curved track from the intermediate position ASB0 to the set position A3z. Preferably, the straight track from the second position A2 to the intermediate position ASB0 overlaps with the detection track R1a.

[0057] It should be noted that, although the illustration is omitted, the first path R1 may also include a track from the starting position of the moving body 10 to the first position A1.

[0058] However, the first path R1 is not limited to the track described above. For example, the first path R1 may not include the approach track R1b. That is, the first path R1 may include at least the detection track R1a from the first position A1 to the second position A2. Furthermore, the first path R1 is not limited to a track that traverses the setting area AR0 (target object P) like the detection track R1a; for example, it may be a track that approaches the front surface of the setting area AR0 (target object P) in a straight line. Furthermore, the first path R1 is not limited to the moving body 10 moving in a straight line in the rearward direction, traversing the setting area AR0 (target object P) and turning back at the second position A2; it may also be a track that moves in a straight line in the forward direction and turns back to approach the front surface of the setting area AR0 (target object P).

[0059] The first path R1 is preset based on the map information of device W. The map information of device W includes information such as the location of obstacles (pillars, etc.) set on device W and the paths that the mobile body 10 can travel on; it can be said to represent the area that the mobile body 10 can move within area AR. In addition to the map information of device W, the first path R1 can also be set based on the vehicle specifications of the mobile body 10. Vehicle specifications include, for example, the size of the mobile body 10, the minimum turning radius, and other specifications that affect the path that the mobile body 10 can move on. When the first path R1 is set based on the vehicle specifications, the first path R1 can be set for each mobile body. It should be noted that the first path R1 can be set by a person based on map information, vehicle specifications, etc., or it can be set automatically by a device such as information processing device 14 based on map information, vehicle specifications, etc. When the first path R1 is set automatically, for example, a desired waypoint can be specified. In this case, the first path R1 that passes through the desired waypoint and is the shortest while avoiding obstacles (walls, etc.) can be set.

[0060] It should be noted that the first path acquisition unit 54 may also set the first path R1 by calculation instead of reading the preset first path R1. In this case, the first path acquisition unit 54 may generate a path as the first path R1 from the current position of the mobile body 10, through the first position A1 and the second position A2 to the mobile destination, i.e., the set position A3z, based on the position information of the mobile body 10, the position information of the setting area AR0, and the map information of the device W.

[0061] The information processing device 14 transmits the information of the first path R1 obtained to the mobile body 10, which is the target, via the communication unit 40. The first path R1 is a path toward the setting area AR0, and therefore can be said to be information related to the movement of the mobile body 10.

[0062] (Control device for the moving body)

[0063] Next, the control device 28 of the moving body 10 will be described. Figure 5 This is a schematic block diagram of the control device for the moving body 10. The control device 28 controls the moving body 10. The control device 28 moves the moving body 10 along a second path R2 set based on multiple detection results from the sensor 26 of the moving body 10 to the target position A3, and causes the moving body 10 to pick up the target object P. The control device 28 is a computer, such as... Figure 5As shown, it includes a communication unit 60, a storage unit 62, and a control unit 64. The communication unit 60 is a module used by the control unit 64 and communicates with external devices such as the information processing device 14; for example, it may include an antenna. In this embodiment, the communication method of the communication unit 60 is wireless communication, but the communication method can be arbitrary. The storage unit 62 is a memory that stores various information such as the calculation content and programs of the control unit 64; for example, it includes at least one of a main storage device such as RAM or ROM, and an external storage device such as an HDD.

[0064] The control unit 64 is a computing device, i.e., a CPU. The control unit 64 includes a first path information acquisition unit 70, a movement control unit 72, a detection control unit 74, a target information acquisition unit 76, a reliability information acquisition unit 78, a selection information acquisition unit 80, and a second path information acquisition unit 82. The control unit 64 reads and executes programs (software) from the storage unit 62 to realize the first path information acquisition unit 70, the movement control unit 72, the detection control unit 74, the target information acquisition unit 76, the reliability information acquisition unit 78, the selection information acquisition unit 80, and the second path information acquisition unit 82, and performs their processing. It should be noted that the control unit 64 can execute these processes using a single CPU, or it can have multiple CPUs and execute the processes using those multiple CPUs. Alternatively, at least a portion of the first path information acquisition unit 70, the movement control unit 72, the detection control unit 74, the target information acquisition unit 76, the reliability information acquisition unit 78, the selection information acquisition unit 80, and the second path information acquisition unit 82 can be implemented using hardware circuitry. In addition, the program used by the control unit 64 stored in the storage unit 62 can also be stored in a recording medium readable by the control device 28.

[0065] (First Path Information Acquisition Department)

[0066] The first path information acquisition unit 70 acquires information about the first path R1. When the mobile body 10 is selected as the work target, the first path information acquisition unit 70 can acquire information about the first path R1 from the information processing device 14, or it can read information about the first path R1 stored in the pre-storage unit 62.

[0067] (Mobility Control Department)

[0068] The motion control unit 72 controls the movement mechanisms of the moving body 10, such as the drive unit and steering wheel, to control the movement of the moving body 10. The motion control unit 72 causes the moving body 10 to move along the first path R1 obtained by the first path information acquisition unit 70 and the second path R2 obtained by the subsequent second path information acquisition unit 82. The motion control unit 72 moves the moving body 10 by sequentially acquiring its position information via the first path R1 and the second path R2. The method for acquiring the position information of the moving body 10 is arbitrary. For example, in this embodiment, a detection body (not shown) is provided in the device W, and the motion control unit 72 acquires the position and attitude information of the moving body 10 based on the detection of the detection body. Specifically, the moving body 10 irradiates a laser towards the detection body and receives the reflected light of the laser reflected by the detection body, thereby detecting its own position and attitude in the device W. Here, the position of the moving body 10 is the coordinate in a two-dimensional coordinate system CO in the region A of the device W, with directions X and Y. Unless otherwise stated, the position refers to the coordinate in the two-dimensional coordinate system CO. Furthermore, the orientation of the moving body 10 is the direction (rotation angle) of the moving body 10 when viewed from the direction Z, which is orthogonal to the directions X and Y. In addition, the method for obtaining the position and orientation information of the moving body 10 is not limited to using a detector; for example, self-positioning techniques such as SLAM (Slmultaneous Localization and Mapping) can also be used.

[0069] (Inspection and Control Department)

[0070] The detection control unit 74 causes the sensor 26 to detect the front surface Pa of the target object P, and obtains the detection result of the sensor 26 as a point group. The specific processing of the detection control unit 74 will be described later.

[0071] (Target location information acquisition unit)

[0072] The target information acquisition unit 76 acquires the estimated position and attitude of the target P based on the position estimation of the point group. The specific processing of the target information acquisition unit 76 will be described later.

[0073] (Reliability Information Acquisition Department)

[0074] The reliability information acquisition unit 78 acquires reliability information regarding the accuracy of the estimated position and attitude of the target object P. The specific processing by the reliability information acquisition unit 78 will be described later.

[0075] (Select Information Acquisition Department)

[0076] The selection information acquisition unit 80 acquires information based on reliability selection from multiple estimation results of the position and attitude of the target object P. The specific processing of the selection information acquisition unit 80 will be described later.

[0077] (Second Path Information Acquisition Department)

[0078] The second path information acquisition unit 82 acquires information about the second path R2, which is set based on the selected estimation result, up to the target position A3. The specific processing of the second path information acquisition unit 82 will be described later.

[0079] (Processing by the control device)

[0080] Next, the processing of the control device 28 when the moving body 10 is toward the target object P will be explained.

[0081] (Move along the first path)

[0082] The movement control unit 72 of the control device 28 causes the moving body 10 to move along the first path R1 obtained by the first path information acquisition unit 70. The movement control unit 72 causes the moving body 10 to move from its current position to the second position A2 via the first position A1 by means of the first path R1.

[0083] (Acquiring a cluster of dots)

[0084] Figure 6 This is a schematic diagram illustrating the detection of the target object. For example... Figure 6As shown, during the movement of the moving body 10 along the first path R1 between the first position A1 and the second position A2, i.e., during the movement on the detection track R1a, the detection control unit 74 causes the sensor 26 to repeatedly detect the front surface Pa of the target object P. The detection control unit 74 detects the front surface Pa of the target object P by directing the sensor 26 toward the known setting area AR0. Specifically, the detection control unit 74 directs the sensor 26 to irradiate the laser LT toward the setting area AR where the target object P is located. The laser LT is reflected by the front surface Pa of the target object P, and the sensor 26 receives the reflected light from the front surface Pa of the target object P. Based on the detection result of the reflected light received by the sensor 26, the detection control unit 74 obtains a point group M0, which is a set of measurement points M. The measurement point M is a point representing the position (coordinates) of the reflected laser LT, and the point group M0 is the set of points representing the position of the reflected laser LT. In this embodiment, the detection control unit 74 calculates the position (coordinates) of the part reflecting the reflected light in the two-dimensional coordinate system CO in the X and Y directions as the measurement point M based on the detection result of the reflected light and the position of the moving body 10. However, the detection control unit 74 is not limited to using the position in the two-dimensional coordinate system CO as the measurement point M, and may also use the position in the coordinate system based on the sensor 26 and the moving body 10 as the measurement point M.

[0085] (Determining the position and orientation of the target object)

[0086] The target information acquisition unit 76 estimates the position and orientation of the target object P based on the measurement points M included in the point group M0 obtained by a single detection (a single scan of the laser LT) by the sensor 26. In this embodiment, the position and orientation of the target object P are estimated by detecting the approximate lines L of the measurement points M obtained by the single detection by the sensor 26. The approximate lines L correspond to the front surface Pa of the target object P; that is, for example, the inclination of the approximate lines L in the two-dimensional coordinate system CO corresponds to the orientation of the target object P, and the position of the approximate lines L in the two-dimensional coordinate system CO corresponds to the position of the target object P. Hereinafter, an example of the detection process of the approximate lines L will be specifically described.

[0087] (Obtaining Approximate Line Candidates)

[0088] Figure 7 This is a schematic diagram used to illustrate the acquisition of approximate line candidates. For example... Figure 7As shown, the target object information acquisition unit 76 sets the detection target area ROIa based on the position of the setting area AR0. The detection target area ROIa is a so-called ROI (Regions of Interest). Based on the position of the setting area AR0, the target object information acquisition unit 76 determines the area presumed to contain the front surface Pa of the target object P, and sets this area as the detection target area ROIa. For example, the target object information acquisition unit 76 sets the detection target area ROIa with a width larger than the setting area AR0, and extracts the measurement points M within the detection target area ROIa from the measurement points M contained in the point group M0 obtained by a single detection by the sensor 26. In order to detect the approximate line L0 corresponding to the front surface Pa of the target object P, the target object information acquisition unit 76 obtains one or more approximate line candidates. For each of the one or more approximate line candidates, the target object information acquisition unit 76 assigns a constant score with a lower selection priority than other measurement points M to the measurement points M that are closer to the front side than a predetermined distance from the approximate line candidate. The target object information acquisition unit 76 selects an approximate line L0 corresponding to the front surface Pa of the target object P from one or more approximate line candidates based on the cumulative score obtained from the cumulative score of the measurement points M within the detected target area ROIa. It should be noted that in this embodiment, since the front surface Pa of the target object P is planar, the approximate line candidates, approximate line L0, and approximate line L are detected as straight lines, but these are not limited to straight lines. For example, if the front surface Pa is known to be curved, the approximate line candidates, approximate line L0, and approximate line LN can also be calculated as curves accordingly.

[0089] Figure 8 This is a schematic diagram illustrating a specific example of a candidate approximation line. For example, such as... Figure 8 As shown, the target information acquisition unit 76 can further set a detection target area ROIb with a width narrower than the detection target area ROIa, and obtain the straight line connecting two measurement points M selected from the point group M0 in the detection target area ROIb as a candidate approximate line. Figure 8 The example shown illustrates how the target information acquisition unit 76 acquires four approximate line candidates L1, L2, L3, and L4 based on the point group M0 in the detected target area ROIb. The detected target area ROIb is preferably set to a width such that adjacent goods or the like will not enter it. The width of the detected target area ROIb can be larger or smaller than the set area AR0.

[0090] Here, a specific example of the method for obtaining approximate line candidates will be described. The target object information acquisition unit 76 may also be configured to obtain a straight line connecting two measurement points M selected from a group of points M0 within the detected object region using the RANSAC algorithm as an approximate line candidate. In this case, the detected object region is the detected object region ROIb, but it could also be the detected object region ROIa, or a region different from the detected object regions ROIa and ROIb. RANSAC (RANdom Sample Consensus) is a method for randomly selecting two points to calculate an evaluation value. It should be noted that the method for obtaining approximate line candidates in this invention is not limited to the method using RANSAC; other algorithms such as PROSAC (Progressive Sample Consensus) can also be used.

[0091] The target information acquisition unit 76 can also be configured to acquire a straight line connecting two measurement points M selected from each of the point groups M0 on the right and left sides of the center of the detected target area in the width direction as a candidate approximate line. In this case, since the number of combinations is limited, any combination of two points can also be used as a candidate approximate line.

[0092] (Detection of approximate line L0)

[0093] The target object information acquisition unit 76 calculates a cumulative score for each of the more than one approximate line candidates, and detects an approximate line L0 corresponding to the front surface Pa of the target object P based on the cumulative score. The target object information acquisition unit 76 selects (i.e. detects) a group of points M0 outside the detection object region ROIb but within the detection object region ROIa, which are high-priority approximate line candidates represented by the cumulative score, as approximate line L0. Such a group of points M0 can also be used as objects for subsequent processing. Then, as an example, the target object information acquisition unit 76 selects... Figure 8 The approximate line candidate L1 shown is taken as the approximate line L0.

[0094] Here, the concept of cumulative score is explained. For example, when prioritizing approximate lines with high cumulative score candidates as the lines used in line segment detection, "the score of the low-priority constant" refers to a score that decreases the cumulative score. On the other hand, for example, when prioritizing approximate lines with low cumulative score candidates, "the score of the low-priority constant" refers to a score that increases the cumulative score.

[0095] In calculating the cumulative score, the target object information acquisition unit 76 utilizes the vertical distance between each measurement point M constituting the point group M0 and the candidate approximation line. Specifically, the target object information acquisition unit 76 assigns a score to measurement points M that are less than a predetermined distance from the candidate approximation line: measurement points M that are closer to the candidate approximation line (on the side of the moving body 10 at the time of detection) than the predetermined distance from the candidate approximation line have higher priority, and the closer they are to the candidate approximation line, the higher their priority. For example, the target object information acquisition unit 76 assigns a negative constant score to measurement points M that are closer to the candidate approximation line than the predetermined distance, and a positive score to measurement points M that are less than the predetermined distance. The value of the positive score is a variable that depends on the distance from the candidate approximation line.

[0096] Figure 9 This is a graph illustrating an example of a fraction. The horizontal axis of the graph corresponds to the Y-direction (depth) and represents the distance between the measuring point M and the approximate line candidate, meaning that the further to the right of the horizontal axis, the deeper the line (the side furthest from the position of the moving body 10 during detection). The position where the depth is zero is the position of the approximate line candidate. The vertical axis represents the assigned fraction value. Figure 9 In the example shown, a negative constant fraction is assigned to the measurement point M that is closer to the candidate approximation line than a specified distance away. Thus, a certain penalty is applied to the measurement point M that is closer to the candidate approximation line than a specified distance away, regardless of the distance between the candidate approximation line and the measurement point M. For the measurement point M that is closer to the candidate approximation line than a specified distance away, a larger positive fraction is assigned.

[0097] In calculating the cumulative score, the target information acquisition unit 76 excludes measurement points M on the depth side that are more than a specified distance from the candidate approximate line from the cumulative score collection. "Excluded from the cumulative score collection" means that these measurement points M are substantially excluded from the cumulative score collection; they may be assigned a score of zero or not assigned any score. Figure 9 In the example shown, the measurement point M on the depth side above the candidate distance of the approximate line is assigned a fraction of zero. When such a method of calculating cumulative fractional values ​​is used, in Figure 8 In the example shown, candidate approximation line L1 is selected as approximation line L0. It should be noted that the target information acquisition unit 76 can also detect multiple approximation lines with high priority represented by the cumulative score value.

[0098] (Determining the two endpoints)

[0099] The target object information acquisition unit 76 determines the positions of the two endpoints of the target object P in the width direction and detects the line segments divided by the approximate line L0 at the two endpoints as the approximate line L. Specifically, the target object information acquisition unit 76 extracts one or more line segments from the approximate line L0 and searches for one or more line segments from the extracted line segments that have a length corresponding to the lengths of the two ends of the target object P determined according to the design information. The "corresponding length" refers to the length within the tolerance range. The design information of the target object P is information indicating the dimensions of the target object P (e.g., the lengths of the two ends, the position and spacing of the openings PB, etc.), and is read, for example, from the storage unit 62 of the control device 28. The target object information acquisition unit 76 determines the two ends of the found line segment or combination of two or more line segments as the two endpoints of the target object P in the width direction. It should be noted that the two endpoints of the target object P refer to the endpoints on one side and the endpoints on the other side of the front surface Pa of the target object P in the width direction.

[0100] Figure 10 This is a schematic diagram illustrating an example of the process of determining the two endpoints of a target object. For example... Figure 10 As shown, for example, the target information acquisition unit 76 extracts three line segments (measurement points M surrounded by circles) from the selected approximate line L0, and searches for a combination of one or more line segments with a length WD at both ends corresponding to the length between the two endpoints of the target object P. Figure 10 In the example shown, the search result is that the lengths of the two ends of the combination of three line segments are detected.

[0101] (Detection of approximate line L)

[0102] After determining the positions of the two endpoints of the target object P, the target object information acquisition unit 76 performs line fitting using the least squares method for each measurement point M to calculate an approximate line L. Specifically, the target object information acquisition unit 76 calculates the approximate line L based on the line segments detected at both ends of the target object P. Among each measurement point M (measurement points M obtained through a single detection based on sensor 26), the target object information acquisition unit 76 excludes measurement points M whose positions are presumed to be the opening PB of the target object P, and excludes measurement points M located at a predetermined distance from both ends of the line segment. Only the remaining measurement points M are used to perform line fitting using the least squares method to calculate the approximate line L. The estimation of the position of the opening PB of the target object P is based on the detected two endpoints of the target object P and the design information of the target object P.

[0103] (Estimation of the position and attitude of the target object based on approximate line L)

[0104] The target information acquisition unit 76 estimates the position and attitude of the target object P based on the approximation line L. The target information acquisition unit 76 estimates the attitude of the target object P based on the inclination of the approximation line L, and estimates the position of the target object P based on a reference position on the approximation line L (e.g., the midpoint of the approximation line L).

[0105] It should be noted that the detection method for the approximate line L is not limited to the method described above. The approximate line L can also be detected using any method based on the acquired measurement points M. For example, the target information acquisition unit 76 may not perform the determination of the two endpoints, but instead perform linear fitting using the least squares method on all measurement points M acquired through a single detection to calculate the approximate line L. Furthermore, the method for estimating the position and attitude of the target object P is not limited to the method using the approximate line L. The position and attitude of the target object P can also be estimated using any method based on the acquired measurement points M.

[0106] (Multiple tests)

[0107] During the movement of the moving body 10 along the first path R1 (detection track R1a), the detection control unit 74 causes the sensor 26 to perform multiple detections of the front surface Pa of the target object P. In other words, during the movement of the moving body 10 along the first path R1 (detection track R1a), the detection control unit 74 causes the sensor 26 to perform multiple laser LT scans of the front surface Pa of the target object P. The detection control unit 74 obtains the detection result of each detection by the sensor 26 as a point group M0. Based on each detection result of the sensor 26 (each time the sensor 26 performs a detection), the target object information acquisition unit 76 estimates the position and orientation of the target object P. That is, for each detection by the sensor 26, the target object information acquisition unit 76 obtains the estimated position and orientation of the target object P.

[0108] Thus, in this embodiment, during movement along the first path R1, the sensor 26 performs multiple detections. That is, the detection control unit 74 causes the sensor 26 to detect each position of the moving body 10 along the first path R1, so the position of the moving body 10 at each detection result is different. However, the timing of sensor 26 detection is not limited to the movement along the first path R1. For example, sensor 26 can also perform detection when the moving body 10 is stopped. Furthermore, the detection control unit 74 is not limited to causing one sensor 26 to perform multiple detections; it can also cause multiple detections by causing multiple sensors 26 to detect the same target object P.

[0109] (Reliability calculation)

[0110] The reliability information acquisition unit 78 calculates the reliability of the estimation results regarding the position and attitude of the target object P estimated by the target object information acquisition unit 76, in terms of estimation accuracy. That is, reliability is an index representing the degree of accuracy of the estimation of the position and attitude of the target object P. In this embodiment, it can be said that the higher the reliability, the higher the estimation accuracy of the position and attitude of the target object P. The reliability information acquisition unit 78 calculates the reliability for each estimation result, that is, for each detection (scan) of the sensor 26. The method for calculating the reliability will be explained below.

[0111] (Calculation of the ideal number of measuring points)

[0112] Figure 11 This is a schematic diagram illustrating the reliability calculation method. The reliability information acquisition unit 78 calculates the reliability of the estimated position and attitude of the target object P based on the ratio of the ideal number of measurement points M (ideal number of point groups) obtained in a single detection by the sensor 26 to the actual number of measurement points M (number of measurement points M (point groups) that measured the front surface Pa of the target object P) obtained in a single detection by the sensor 26. The ideal number of measurement points M is the number of measurement points M observable on the front surface Pa of the target object P in a single detection by the sensor 26, under ideal conditions where the effects of measurement errors are not considered. The reliability information acquisition unit 78 calculates the ideal number of measurement points M based on the estimated position and attitude of the target object P, the position of the moving body 10 during the detection, and the performance of the sensor 26. The performance of the sensor 26 refers to at least one of the following: the angular resolution of the sensor 26, the measurement speed (cycle) of the sensor 26, and the number of measurement points of the sensor 26. It should be noted that angular resolution refers to the angle at which two object points can be detected separately. Furthermore, the position of the moving body 10 can be either a reference position for the moving body 10 or the position of the sensor 26 that has been detected. When the position of the moving body 10 is set to the position of the sensor 26 that has been detected, the reliability information acquisition unit 78 can determine the position of the moving body 10 based on the position of the moving body 10 at the time of detection and the position of the sensor 26 relative to the moving body 10. The position of the sensor 26 relative to the moving body 10 can, for example, be obtained in advance as design information.

[0113] Here, the angle at which the sensor 26 scans the laser LT in a single detection is defined as the scanning angle. In this embodiment, the reliability information acquisition unit 78, based on the estimated position and orientation of the target object P and the position of the moving body 10 during detection, calculates the range of angles at which the laser LT illuminates the front surface Pa of the target object P within the scanning angle as the detection angle θ, when the sensor 26 has performed detection from the reference position AR1. That is, the reliability information acquisition unit 78 excludes the range of angles at which the laser LT illuminates the front surface Pa of the target object P, and extracts the range of angles at which the laser LT illuminates the front surface Pa of the target object P as the detection angle θ. For example, the reliability information acquisition unit 78 calculates the detection angle θ based on the angle formed by the straight line connecting the point on the front surface Pa determined based on the estimated position and orientation of the target object P to the reference position AR1, and the straight line connecting the other points on the front surface Pa determined based on the estimated position and orientation of the target object P to the reference position AR1. It should be noted that the reference position AR1 is the position corresponding to the position of the moving body 10 when the detection was performed. In this embodiment, it is the position of the moving body 10 itself when the detection was performed.

[0114] More specifically, the reliability information acquisition unit 78 calculates the detection angle θ for each column PA. For example... Figure 11 As shown, the reliability information acquisition unit 78, based on the estimated position and attitude of the target object P (approximate line L), acquires the positions of the two endpoints of the column PA, and calculates the angle θ between the straight line LA1 connecting the reference position AR1 to one endpoint PS1 of the column PA, and the straight line LA2 connecting the reference position AR1 to the other endpoint PS2 of the column PA. Figure 11 In the example, the reliability information acquisition unit 78 calculates the detection angle θ1 of column PA1, the detection angle θ2 of column PA2, and the detection angle θ3 of column PA3.

[0115] The reliability information acquisition unit 78 calculates the number of ideal measurement points M based on the calculated ratio of the detection angle θ to the angular resolution of the sensor 26. More specifically, the reliability information acquisition unit 78 calculates the number of ideal measurement points M as the value obtained by dividing the detection angle θ by the angular resolution. In this embodiment, the reliability information acquisition unit 78 calculates the number of ideal measurement points M for each column PA. Figure 11In the example, the reliability information acquisition unit 78 calculates the value obtained by dividing the detection angle θ1 about column PA1 by the angular resolution as the number of ideal measurement points M in column PA1 (ideal number of measurement points NM1), calculates the value obtained by dividing the detection angle θ2 about column PA2 by the angular resolution as the number of ideal measurement points M in column PA2 (ideal number of measurement points NM2), and calculates the value obtained by dividing the detection angle θ3 about column PA3 by the angular resolution as the number of ideal measurement points M in column PA3 (ideal number of measurement points NM3).

[0116] (Calculation of the actual number of measurement points)

[0117] The reliability information acquisition unit 78 calculates the number of actual measurement points M. The number of actual measurement points M refers to the measurement points M that were measured on the front surface Pa of the target object P from among the measurement points M obtained in a single detection by the sensor 26. For example, the reliability information acquisition unit 78 extracts measurement points M that are within a predetermined distance range relative to the front surface Pa (here, the approximate line L) of the target object P, determined based on the estimated position and orientation of the target object P, from among the measurement points M obtained in a single detection by the sensor 26, and counts them as actual measurement points M. Alternatively, the reliability information acquisition unit 78 may also use the measurement points M used in the detection of the approximate line L as actual measurement points M.

[0118] More specifically, the reliability information acquisition unit 78 calculates the number of actual measurement points M for each pillar PA. From the measurement points M acquired in a single detection by the sensor 26, the reliability information acquisition unit 78 extracts measurement points M that are within a specified distance range relative to the estimated position of the pillar PA (the position of the pillar PA determined based on the estimated position and attitude of the target object P) as the actual measurement points M for that pillar PA, and counts their number. Figure 11 In the example, the reliability information acquisition unit 78 calculates the number of measurement points M within a predetermined area ARX1 located near the presumed position of pillar PA1 as the actual number of measurement points M measured on pillar PA1 (actual measurement point count nm1). Similarly, the reliability information acquisition unit 78 calculates the number of measurement points M within a predetermined area ARX2 located near the presumed position of pillar PA2 as the actual number of measurement points M measured on pillar PA2 (actual measurement point count nm2). Similarly, the reliability information acquisition unit 78 calculates the number of measurement points M within a predetermined area ARX3 located near the presumed position of pillar PA3 as the actual number of measurement points M measured on pillar PA3 (actual measurement point count nm3).

[0119] (Reliability calculation based on the ideal number of measurement points and the actual number of measurement points)

[0120] The reliability information acquisition unit 78 calculates the reliability of the estimated position and attitude of the target object P based on a comparison between the ideal number of measurement points M and the actual number of measurement points M. In this embodiment, the reliability information acquisition unit 78 calculates the reliability based on the ratio of the actual number of measurement points M to the ideal number of measurement points M. The reliability information acquisition unit 78 calculates the reliability in a manner that the smaller the difference between the ideal number of measurement points M and the actual number of measurement points M, the higher the reliability.

[0121] In this embodiment, the reliability information acquisition unit 78 calculates the reliability for each pillar PA. The reliability information acquisition unit 78 calculates the ratio of the actual number of measurement points M for the pillar PA to the ideal number of measurement points M for that pillar PA, as the reliability of the estimation result for that pillar PA. Furthermore, based on the reliability of each pillar PA, the reliability information acquisition unit 78 calculates the reliability of the estimation result regarding the position and attitude of the target object P, which is estimated through a single detection based on sensor 26. In this embodiment, the reliability information acquisition unit 78 uses the minimum reliability among all pillar PAs as the reliability of the estimation result regarding the position and attitude of the target object P. That is, in... Figure 11 In the example, if the reliability of the estimated position and attitude of the target object P in a single detection is set as S, the reliability S is calculated using the following formula (1).

[0122] S=min(nm1 / NM1, nm2 / NM2, nm3 / NM3)···(1)

[0123] For each estimated result of the position and attitude of the target object P, that is, for each detection (scan) of the sensor 26, the reliability information acquisition unit 78 calculates the reliability S in the same way.

[0124] (Another example of reliability calculation)

[0125] The reliability information acquisition unit 78 calculates reliability using the methods described above, but the method for calculating reliability is not limited to the above description, and any method can be used. For example, in the above description, the reliability information acquisition unit 78 calculates the reliability of the measurement point M for each column PA, and calculates the reliability of the estimated result of a single test based on the reliability of each column PA, but is not limited to this. In this case, for example, the reliability information acquisition unit 78 calculates the number of ideal measurement points M for each column PA using the same method as described above, and sums them up, thereby calculating the total number of ideal measurement points for all columns PA. Furthermore, the reliability information acquisition unit 78 calculates the number of actual measurement points M for each column PA using the same method as described above, and sums them up, thereby calculating the total number of actual measurement points M. Furthermore, the reliability information acquisition unit 78 can calculate the ratio of the total number of actual measurement points M to the total number of ideal measurement points M as the reliability of the estimated result of a single test.

[0126] (Selection of presumed results)

[0127] The selection information acquisition unit 80 selects the estimation result for setting the second path R2 from the estimation results of each detection of the sensor 26 calculated by the reliability information acquisition unit 78, based on the reliability of the estimation results of each detection of the sensor 26. Specifically, the selection information acquisition unit 80 selects the estimation result with high reliability as the estimation result for setting the second path R2. For example, the measurement point M obtained by the first detection of the sensor 26 is designated as the first measurement point (first point group), and the measurement point M obtained by the second detection of the sensor 26, which is different from the first detection, is designated as the second measurement point (second point group). In this case, the target object information acquisition unit 76 estimates the position and attitude of the target object based on the first measurement point, and the reliability information acquisition unit 78 calculates the reliability of the estimation result (based on the estimation result of the first measurement point). Similarly, the target object information acquisition unit 76 estimates the position and attitude of the target object estimated based on the second measurement point, and the reliability information acquisition unit 78 calculates the reliability of the estimation result (based on the estimation result of the second measurement point). In this case, the selection information acquisition unit 80 selects the more reliable estimation result from the estimation results based on the first measurement point and the estimation results based on the second measurement point as the estimation result for setting the second path R2.

[0128] Furthermore, the information acquisition unit 80 selects the estimation result with the highest reliability from among the various estimation results as the estimation result used for setting the second path R2. However, the method for selecting the estimation result is not limited to the above method, and the estimation result can also be selected by any method based on the reliability of each estimation result.

[0129] (Setting up the second path)

[0130] Figure 12 This is a schematic diagram illustrating an example of movement along a second path. The second path information acquisition unit 82 sets a target position A3 based on the estimated position and orientation of the target object P selected by the selection information acquisition unit 80. For example, the second path information acquisition unit 82 calculates the position and orientation at which the target object P can be picked up (the fork 24 can be inserted into the opening PB of the target object P by moving in a straight line) based on the estimated position and orientation of the target object P selected by the selection information acquisition unit 80, and sets it as the target position A3. As an example, the location where the fork 24 is moved 1000 mm parallel to the axis of the opening PB of the target object P from the entrance of the opening PB can also be set as the target position A3. Then, as... Figure 12 As shown, the second path information acquisition unit 82 sets the track from the second position A2 (which is the starting position) to the set target position A3 as the second path R2. The second path information acquisition unit 82 can also set the second path R2 in any way based on the information of the target position A3, but for example, the second path R2 can be calculated by model predictive control (MPC).

[0131] (Move along the second path)

[0132] The movement control unit 72 moves the moving body 10 from the second position A2 to the target position A3 via the second path R2. Upon reaching the target position A3, the movement control unit 72 causes the moving body 10 to advance linearly from the target position A3 and inserts the forks 24 into the opening PB of the target object P to pick it up. The movement control unit 72 then transports the moving body 10, having picked up the target object P, to the designated transport destination.

[0133] Thus, the motion control unit 72 moves the mobile body 10 from the second position A2 to the target position A3 along the second path R2. However, it is not limited to this; for example, it can switch between movement along the second path R2 and movement based on direct feedback control to move the mobile body 10 to the target position A3. As a direct feedback-based control, for example, a vision servo-based control method described in "Jun Osato and Noriaki Maru, 'Position and Attitude Control of an Omnidirectional Mobile Robot Based on Linear Vision Servo,' Proceedings of the Japanese Society of Mechanical Engineers (C), Vol. 77, No. 774, pp. 215-224, February 25, 2011" can be used. For another example, after the mobile body 10 reaches the target position A3 along the second path R2, the motion control unit 72 can switch to direct feedback-based control to move the mobile body 10 by picking up the target object P.

[0134] (Process flow for target position and attitude detection)

[0135] Next, the processing flow for setting the second path as described above will be explained. Figure 13 This is a flowchart illustrating the processing flow of the second path setting. During the movement of the moving body 10 along the first path R1 from the first position A1 to the second position A2, the detection control unit 74 of the control device 28 causes the sensor 26 to detect the target object P multiple times. That is, in this embodiment, the sensor 26 detects the target object P while moving along the first path R1. Figure 13 As shown, the detection control unit 74 acquires a point group M0 (measurement point M) based on a single detection result from the sensor 26 (step S10). Then, the target information acquisition unit 76 of the control device 28 estimates the position and orientation of the target object P based on the point group M0 (step S12). Then, the reliability information acquisition unit 78 of the control device 28 calculates the number of ideal measurement points M in the detection based on the estimated position and orientation of the target object P, the position of the moving body 10 during the detection, and the performance of the sensor 26 during the detection (step S14), and calculates the reliability of the estimated position and orientation of the target object P in the detection based on the ratio of the number of ideal measurement points M to the actual number of measurement points M (step S16).

[0136] Then, the control device 28 determines whether the number of calculated estimation results is above a threshold. If it is not determined to be above the threshold (step S18; No), that is, if the number of estimation results is determined to be less than the threshold, it returns to step S10 and obtains the point group M0 based on other detection results (detection results based on other scans) from the sensor 26. The estimation results and reliability are then repeatedly calculated. It should be noted that the threshold can be arbitrarily set here. If the number of calculated estimation results is above the threshold (step S18; Yes), the selection information acquisition unit 80 of the control device 28 selects the estimation result with the highest reliability from the calculated estimation results as the estimation result used for setting the second path R2 (step S20). Then, the second path information acquisition unit 82 of the control device 28 uses the selected estimation result to set the second path R2 (step S22).

[0137] It should be noted that in this embodiment, the position and orientation of the target object P are estimated and the reliability is calculated each time the sensor 26 performs a detection. For example, when the moving body 10 reaches the second position A2 and begins to set the second path R2, the estimated result is selected based on the reliability. However, the timing of each process is not limited to this and is arbitrary. For example, the position and orientation of the target object P can be estimated and the reliability is calculated after the sensor 26 performs any number of detections. Furthermore, when the moving body 10 reaches the second position A2 and begins to set the second path R2, the position and orientation of the target object P can be estimated, the reliability can be calculated, and the estimated result can be selected based on the above detection results.

[0138] (Effect)

[0139] As explained above, in this embodiment, the control device 28 calculates the reliability of the estimated position and orientation of the target object P based on each detection of the sensor 26. Therefore, for example, based on the reliability, the estimated result for setting the second path R2 can be selected, and a high-accuracy second path R2 that can appropriately approach the target position can be set. Therefore, according to this embodiment, the moving body 10 can be appropriately moved to the target position. Furthermore, the control device 28 calculates the reliability based on the ratio of the ideal number of measurement points M calculated from the estimated result to the actual number of measurement points M. The closer the actual number of measurement points M is to the ideal number of measurement points M, the higher the accuracy of the estimated result is considered, thus improving the reliability. By calculating the reliability in this way, a high-accuracy estimated result can be selected, thereby setting a high-accuracy second path R2.

[0140] It should be noted that, in this embodiment, the control device 28 of the moving body 10 estimates the position and attitude of the target object P based on the point group M0, calculates the reliability of the estimation result, selects the estimation result for setting the second path R2 based on the reliability, and sets the second path R2 based on the selected estimation result. However, these processes are not limited to being performed by the moving body 10. For example, at least a portion of these processes can be performed by other devices such as the information processing device 14, and the moving body 10 obtains the results. That is, for example, the target object information acquisition unit 76 of the moving body 10 can obtain the estimation result of the position and attitude of the target object P, and can estimate the position and attitude of the target object P itself, or obtain the estimation result of the position and attitude of the target object P from an external device. As another example, the reliability information acquisition unit 78 of the moving body 10 can obtain the reliability information of the estimation result, and can calculate the reliability itself, or obtain the reliability information of the estimation result from an external device. When the reliability information acquisition unit 78 obtains reliability information from an external device, the mobile body 10 does not need to perform calculations using the estimated result, so it can be said that obtaining the estimated result is unnecessary. For example, the selection information acquisition unit 80 of the mobile body 10 can obtain information about the estimated result selected based on reliability. It can select the estimated result itself or obtain the selected estimated result information from an external device. When the selection information acquisition unit 80 obtains the selected estimated result information from an external device, the mobile body 10 does not need to perform selection processing using reliability, so it can be said that obtaining reliability is unnecessary. For example, the second path information acquisition unit 82 of the mobile body 10 can obtain information about the second path R2 set based on the estimated result. It can set the second path R2 based on the estimated result selected itself or obtain the second path R2 information from an external device. When the second path information acquisition unit 82 obtains the second path R2 information from an external device, the mobile body 10 does not need to perform calculations using the selected estimated result, so it can be said that obtaining the selected estimated result information is unnecessary.

[0141] (Second Implementation)

[0142] Next, the second embodiment will be described. In the second embodiment, the reference position AR1 used to calculate the number of ideal measurement points M is different from that in the first embodiment. In the second embodiment, descriptions of parts with the same structure as in the first embodiment are omitted.

[0143] Figure 14 This is a schematic diagram illustrating the method for calculating the ideal number of measurement points in the second embodiment. For example... Figure 14As shown, in the second embodiment, the reliability information acquisition unit 78 sets a position different from the position of the moving body 10 during the detection as a reference position AR1. The reliability information acquisition unit 78 sets the reference position AR1 based on the estimated position and attitude of the target object P and the position of the moving body 10 during the detection. The reliability information acquisition unit 78 calculates the position where the moving body 10 has a predetermined position and attitude relative to the estimated position and attitude of the target object P (here, the approximation line L) and uses this as the reference position AR1. Specifically, the reliability information acquisition unit 78 calculates the reference position AR1 in such a way that the length of the straight line LX connecting the reference position AR1 and the reference point of the approximation line L is equal to the length from the position of the moving body 10 during the detection to the reference point of the approximation line L, and the angle between the straight line LX and the normal of the approximation line L is within a predetermined range. Here, the reference point on the approximation line L can be considered the estimated position of the target object P determined based on the estimated position and attitude of the target object P. The reference point on the approximation line L can be any position, for example, the midpoint of the approximation line L. Furthermore, it is preferable that the angle between the straight line LX and the normal to the approximate line L is small. For example, the reference position AR1 is preferably set such that the angle between the straight line LX and the normal to the approximate line L is smaller than the angle between the straight line connecting the reference point of the approximate line L from the position of the moving body 10 during detection and the normal to the approximate line L. Further, the reference position AR1 is preferably such that the angle between the straight line LX and the normal to the approximate line L is 0 degrees or more and a predetermined angle (e.g., 5 degrees) or less. That is, it is preferable that the reliability information acquisition unit 78 sets a position with a small incident angle of the laser LT, such as 0°, as the reference position AR1.

[0144] In the second embodiment, the position with a smaller incident angle of the laser LT is set as the reference position AR1. The subsequent calculation of the number of ideal measurement points M is the same as in the first embodiment, and therefore the explanation is omitted. Here, when the incident angle of the laser LT from the reference position AR1 is large, the detection angle θ (the range of angles from which the laser LT illuminates the target object P) becomes smaller, thus reducing the number of calculable ideal measurement points M. The number of ideal measurement points M is used as the denominator in the reliability calculation; therefore, if the number of calculable ideal measurement points M decreases, the denominator becomes smaller, resulting in a deviation in reliability and potentially reducing the accuracy of the reliability calculation. In contrast, in the second embodiment, the reference position AR1 is set with a smaller angle between the normal to the line LX and the approximate line L, thus preventing the denominator from becoming a small value in the reliability calculation, thereby suppressing the reduction in the accuracy of the reliability calculation.

[0145] (Another example of reliability calculation)

[0146] The first and second embodiments are similar in that reliability is calculated based on the ratio of the ideal number of measurement points M to the actual number of measurement points M. However, reliability is not limited to being calculated based on the ratio of the ideal number of measurement points M to the actual number of measurement points M; it can also be calculated using estimated results and any other method. Another example of a reliability calculation method will be described below.

[0147] For example, the reliability information acquisition unit 78 can also calculate the reliability based on the residuals of the approximation line L. The residual refers to the distance between the measurement point M and the approximation line L. Preferably, the smaller the residual of the approximation line L, the higher the reliability setting of the reliability information acquisition unit 78. For example, the reliability information acquisition unit 78 can calculate the distance between the measurement point M in the two-dimensional coordinate system CO and the approximation line L as the residual for each measurement point M, and the smaller the total value of the calculated residuals, the higher the reliability setting. For example, the reliability information acquisition unit 78 can calculate the ratio of the total residual value to the total value of the measurement points M as the reliability of the estimated result.

[0148] For example, the reliability information acquisition unit 78 can remove a portion of the measurement points M used in the calculation of the approximate line L, and calculate the reliability of the estimation result based on the difference between the approximate line L' calculated by linear fitting the remaining measurement points M and the approximate line L. For example, the reliability information acquisition unit 78 can calculate the reliability of the estimation result based on the shortest distance between the approximate line L' and the approximate line L. In this case, for example, the shorter the shortest distance between the approximate line L' and the approximate line L, the higher the reliability obtained by the reliability information acquisition unit 78. For another example, the reliability information acquisition unit 78 can calculate the reliability based on the difference between the X coordinate of the approximate line L' and the X coordinate of the approximate line L, the difference between the Y coordinate of the approximate line L' and the Y coordinate of the approximate line L, and the difference between the attitude (angle) of the approximate line L' and the attitude (angle) of the approximate line L. In this case, the smaller these differences, the higher the reliability obtained by the reliability information acquisition unit 78.

[0149] For example, the reliability information acquisition unit 78 can calculate the reliability of the estimation result based on the positional relationship between the moving body 10 and the target object P (approximate line L) during the detection. In this case, for example, the shorter the distance between the moving body 10 and the target object P (approximate line L) during the detection, the higher the reliability obtained by the reliability information acquisition unit 78. For example, the smaller the angle between the straight line connecting the moving body 10 and the target object P (approximate line L) during the detection and the perpendicular line to the approximate line L, the higher the reliability obtained by the reliability information acquisition unit 78.

[0150] For example, the reliability information acquisition unit 78 can calculate the reliability of the estimation result based on the tilt angle of the position of the mobile body 10 on the floor (i.e., the part of the mobile body 10 within the area AR) relative to the horizontal direction. In this case, for example, the reliability information acquisition unit 78 acquires information about the tilt angle of the part of the mobile body 10 at the time of detection relative to the horizontal direction, and calculates the reliability based on the tilt angle. The smaller the tilt angle, the higher the reliability is obtained by the reliability information acquisition unit 78. It should be noted that the tilt angle information can be obtained by any method, for example, a sensor for detecting the tilt angle can be mounted on the mobile body 10, and the detection result of the sensor can be obtained.

[0151] (Third Implementation)

[0152] Next, the third embodiment will be described. The third embodiment differs from the first embodiment in that it uses a method for calculating the reliability when multiple sensors 26 are detected at the same time. In the third embodiment, descriptions of parts with the same structure as in the first embodiment are omitted. It should be noted that the third embodiment can also be applied to the second embodiment.

[0153] (Estimation of the position and orientation of the target object)

[0154] In the third embodiment, the detection control unit 74 causes multiple sensors 26 to perform detection at the same time, i.e., when the moving body 10 is at the same position, and obtains the detection results of each sensor 26 as a point group M0 (measurement point M). Then, the target information acquisition unit 76 overlaps the measurement points M obtained by the multiple sensors 26 at the same time on the same coordinate system (a two-dimensional coordinate system CO in this embodiment). That is, the target information acquisition unit 76 plots each measurement point M obtained by the multiple sensors 26 at the same time on the same two-dimensional coordinate system CO. It should be noted that the coordinate system for overlapping the measurement points M is not limited to the two-dimensional coordinate system CO, and may also be a three-dimensional coordinate system, for example.

[0155] The target information acquisition unit 76 uses overlapping measurement points M to estimate the position and orientation of the target object P using the same method as in the first embodiment. In the third embodiment, multiple sensors 26 perform detection whenever the moving body 10 moves, and the position and orientation of the target object P are estimated each time the moving body 10 moves. In the third embodiment, since the detection results of multiple sensors 26 are overlapped, the number of measurement points M used to estimate the position and orientation of the target object P can be increased, thereby improving the estimation accuracy.

[0156] (Reliability calculation)

[0157] In the third embodiment, an individual reliability is calculated for each sensor 26, and the reliability of the estimated position and attitude of the target object P is calculated based on their individual reliability. The following description uses the case where the first sensor and the second sensor detect at the same time as an example. In this case, the target object information acquisition unit 76 overlaps the measurement points M (point group M0) obtained by the detection of the first sensor (i.e., the first measurement point (first point group)) and the measurement points M (point group M0) obtained by the detection of the second sensor (i.e., the second measurement point (second point group)) on a two-dimensional coordinate system CO. Then, the target object information acquisition unit 76 estimates the position and attitude of the target object P based on the overlapping first and second measurement points. Then, the reliability information acquisition unit 78 calculates the reliability of the detection result of the first sensor (first measurement point), i.e., the first individual reliability, and the reliability of the detection result of the second sensor (second measurement point), i.e., the second individual reliability. The reliability information acquisition unit 78 calculates the reliability of the estimated result calculated based on the overlapping first and second measurement points, based on the first individual reliability and the second individual reliability. It should be noted that the calculation method of the first individual reliability and the second individual reliability can be the same as the reliability calculation method described in the first embodiment, the second embodiment, and another example.

[0158] More specifically, the reliability information acquisition unit 78 calculates the reliability of the estimation result based on each individual reliability and the distance from each sensor 26 to the estimated position of the target object P (e.g., a reference point on the approximate line L). That is, the reliability information acquisition unit 78 calculates the reliability based on a first individual reliability, the distance from the position of the first sensor at the time of detection to the estimated position of the target object P, a second individual reliability, and the distance from the position of the second sensor at the time of detection to the estimated position of the target object P. Specifically, the reliability information acquisition unit 78 weights the individual reliability based on the distance from each sensor to the estimated position of the target object P, and averages the weighted individual reliability to calculate the reliability. The shorter the distance from the sensor 26 to the estimated position of the target object P, the greater the weight assigned by the reliability information acquisition unit 78. For example, the reliability information acquisition unit 78 calculates a first weight for a first individual reliability based on the distance from the position of the first sensor at the time of detection to the estimated position of the target object P, and calculates a second weight for a second individual reliability based on the distance from the position of the second sensor at the time of detection to the estimated position of the target object P. Then, the reliability information acquisition unit 78 divides the sum of the value obtained by multiplying the first individual reliability by the first weight and the value obtained by multiplying the second individual reliability by the second weight by the number of individual reliability values, thereby calculating the reliability of the estimated result.

[0159] It should be noted that the above example illustrates the reliability calculation method using two sensors, but the reliability can also be calculated using the same method when using three or more sensors. That is, for example, the reliability information acquisition unit 78 uses the following equation (2) to calculate the reliability S with respect to the estimated result. In equation (2), n is the number of sensors, v is the individual reliability, and s is the weight.

[0160] S=(v1·s1+v2·s2+···vn·sn) / n···(2)

[0161] In this way, by calculating reliability by giving greater weight to shorter distances from the sensor to the estimated position of the target P, it is possible to more appropriately reflect closer and more accurate detection results in the reliability calculation, and thus, it is possible to appropriately select more accurate estimated results based on reliability. However, the reliability calculation method of the third embodiment is not limited to the above description, and reliability can also be calculated by any method based on the individual reliability of each sensor 26.

[0162] It should be noted that in the first embodiment, multiple sensors 26 may also perform detection at the same time. However, in the case of the first embodiment, the measurement points M detected by each sensor 26 are not overlapped at the same time as in the second embodiment. Instead, the estimation result is calculated separately for each detection of each sensor 26.

[0163] (Fourth Implementation)

[0164] Next, the fourth embodiment will be described. The fourth embodiment differs from the first embodiment in the method for selecting the estimated result used to set the second path R2. In the fourth embodiment, descriptions of parts with the same structure as in the first embodiment are omitted. It should be noted that the fourth embodiment can also be applied to the second and third embodiments.

[0165] In the fourth embodiment, the selection information acquisition unit 80 selects an estimated result for setting the second path R2 based not only on reliability but also on the detection distance, which is the distance from the position of the moving body 10 at the time of detection to the estimated position of the target object P. For example, the selection information acquisition unit 80 selects the estimated result with a reliability of at least a threshold and the shortest detection distance among the estimated results with a reliability of at least the threshold as the estimated result for setting the second path R2. As an example, the case where a first estimated result is calculated based on the measurement point M obtained by the first detection of the sensor 26, a second estimated result is calculated based on the measurement point M obtained by the second detection of the sensor 26, and the reliability of both the first and second estimated results is at least a threshold will be described. In this example, if the first detection distance from the position of the moving body 10 to the estimated position of the target object P in the first detection is shorter than the second detection distance from the position of the moving body 10 to the estimated position of the target object P in the second detection, the first estimated result is selected as the estimated result for setting the second path R2.

[0166] In the fourth embodiment, it is preferable that after generating the second path R2, the estimation result is reselected based on reliability and detection distance as described above, and the second path R2 is updated. That is, in this case, if the moving body 10 reaches the second position A2, the selection information acquisition unit 80 selects the estimation result for setting the second path R2 from the estimation results of the position and attitude of the target object P calculated based on the detection results of the sensor 26 during the movement of the first path R1. Then, the second path information acquisition unit 82 generates the second path R2 using the selected estimation result, and the moving body 10 begins to move along the second path R2. The estimation result for setting the initial second path R2 can be selected based on reliability as in the first embodiment, or it can be selected based on reliability and detection distance as described above.

[0167] Furthermore, the detection control unit 74 also causes the sensor 26 to perform detection as the moving body 10 moves along the second path R2. The target information acquisition unit 76 estimates the position and attitude of the target P based on the measurement point M obtained by the detection of the sensor 26 on the second path R2, and the reliability information acquisition unit 78 calculates the reliability of the estimation result and the detection distance when the detection was performed on the second path R2. Then, the selection information acquisition unit 80 selects the estimation result for updating the second path R2 from the estimation results calculated by the detection of the sensor 26 on the first path R1 and the estimation results calculated by the detection of the sensor 26 on the second path R2 based on the reliability and the detection distance. In this case, as described above, the selection information acquisition unit 80 selects the estimation result with a reliability of ≥10 ... Then, if the estimation result selected for updating differs from the estimation result selected for setting the second path R2, the second path information acquisition unit 82 updates the second path R2 using the estimation result selected for updating. The moving body 10 switches to the updated second path R2 and continues moving. The detection control unit 74 can repeat the update process for the second path R2.

[0168] Thus, in the fourth embodiment, detection and updating of the second path R2 continue during movement along the second path R2, thereby enabling approach with reduced offset relative to the target object P. Furthermore, there is a tendency for higher detection accuracy of the sensor 26 with shorter detection distances; therefore, updating the second path R2 with a shorter detection distance estimate can reduce the offset relative to the target object P. However, in this case, if only a shorter detection distance estimate is selected, it is possible to select an estimate with a short detection distance but a sudden decrease in reliability, potentially leading to offset relative to the target object P. In contrast, in the fourth embodiment, by using a threshold to filter reliability and using an estimate with a certain degree of reliability, it is possible to appropriately approach the target object P.

[0169] (Effects of the invention)

[0170] As explained above, the control method for the mobile body 10 of the present invention is a method for controlling an automatically moving mobile body 10. This method includes the following steps: detecting a target object P using a sensor 26 installed on the mobile body 10, and obtaining the detection result of the sensor 26 as a point group M0; estimating the position and orientation of the target object P based on the point group M0; calculating an ideal number of point groups assumed to be obtained through the detection of the sensor 26 based on the estimated position and orientation of the target object P, the position of the mobile body 10 at the time of detection, and the performance of the sensor 26; and calculating the reliability of the estimated position and orientation of the target object P in terms of accuracy based on a comparison between the ideal number of point groups and the actual number of point groups. According to this control method, by calculating the reliability based on the comparison between the ideal number of point groups and the actual number of point groups, the accuracy of the estimated result can be appropriately controlled. Therefore, for example, the estimated result used for setting the second path R2 can be selected based on the reliability, enabling the mobile body 10 to move appropriately to the target position.

[0171] Furthermore, the sensor 26 detects the target object P by scanning and illuminating light along a predetermined direction. In the step of calculating the ideal number of point clusters, the control method of the present invention, when the sensor 26 illuminates light from a reference position AR1 corresponding to the position of the moving body 10 during detection, calculates the range of angles at which light illuminates the target object P (i.e., the detection angle θ) within the scanning angle of the sensor 26 during a single detection scan, and calculates the ideal number of point clusters based on the ratio of the detection angle θ to the angular resolution of the sensor 26. According to this control method, the number of point clusters detected under appropriate conditions can be calculated as the ideal number of point clusters, thus enabling the reliability to reflect whether the actual detection by the sensor 26 was performed appropriately. Therefore, by using the reliability calculated in this way, the accuracy of the estimated result can be appropriately controlled.

[0172] Furthermore, in the step of calculating the ideal number of point groups, the position of the moving body 10 during the detection is set as the reference position AR1. According to this control method, by using the position of the moving body 10 during the detection as the reference position AR1 to calculate the ideal number of point groups, the reliability can be calculated appropriately, and the accuracy of the estimation result can be appropriately controlled.

[0173] Furthermore, in the step of calculating the ideal number of point clusters, the reference position AR1 is set such that the distance from the reference position AR1 to the estimated position of the target object P is equal to the distance from the position of the moving body 10 during the detection to the estimated position of the target object P, and the angle between the straight line connecting the reference position AR1 to the target object P and the normal of the estimated front surface Pa (approximate line L) of the target object P is within a specified range. According to this control method, by setting the reference position AR1 in this way, it is possible to suppress the decrease in the number of ideal point clusters and suppress the reduction in the accuracy of reliability calculation.

[0174] Furthermore, the target object P is formed by multiple pillars PA and openings PB between the pillars PA. In the step of calculating the ideal number of points, the control method of the present invention, based on the estimated position and attitude of the target object P, determines the position of one end point PS1 and the other end point PS2 in the width direction of each pillar PA. Then, for each pillar PA, the angle formed by the line LA1 connecting the reference position AR1 and the end point PS1 and the line LA2 connecting the reference position AR1 and the end point PS2 is calculated as the detection angle θ, and the ideal number of points for each pillar PA is calculated based on the ratio of the detection angle θ to the angular resolution of the sensor 26. In addition, in the step of calculating reliability, the reliability of each pillar PA is calculated based on the ratio of the ideal number of points for each pillar PA to the actual number of points obtained for each pillar PA, and the reliability of the estimated position and attitude of the target object P is calculated based on the reliability of each pillar PA. According to this control method, by calculating the reliability for each pillar PA in this way, the reliability can be calculated appropriately, and the accuracy of the estimated results can be appropriately controlled.

[0175] Furthermore, in the step of calculating the ideal number of point clusters, the positions of endpoint PS1 on one side and endpoint PS2 on the other side of the column PA in the width direction are determined. Then, for each column PA, the angle θ formed by the straight line LA1 connecting the reference position AR1 and endpoint PS1 and the straight line LA2 connecting the reference position AR1 and endpoint PS2 is calculated as the detection angle. Based on the ratio of the total value of the detection angles θ for each column PA to the angular resolution of the sensor 26, the total value of the ideal number of point clusters for each column PA is calculated. Additionally, in the step of calculating reliability, the reliability is calculated based on the ratio of the total value of the ideal number of point clusters to the actual number of point clusters. According to this control method, the reliability is calculated by summing up the values ​​for each column PA, thus suppressing the computational load.

[0176] Furthermore, in the step of obtaining the detection results of sensor 26 as a point group, the first and second sensors detect the target object P, obtaining a first point group corresponding to the detection results of the first sensor and a second point group corresponding to the detection results of the second sensor. Then, in the step of estimating the position and attitude of the target object P, the first point group and the second point group are overlapped on the same coordinate system, and the position and attitude of the target object P are estimated based on the overlapped first and second point groups. In the step of calculating reliability, the reliability of the first point group (i.e., the first individual reliability) and the reliability of the second point group (i.e., the second individual reliability) are calculated. The reliability is calculated based on the first individual reliability, the distance from the position of the first sensor at the time of detection to the estimated position of the target object P, the second individual reliability, and the distance from the position of the second sensor at the time of detection to the estimated position of the target object P. According to this control method, by increasing the number of point groups used to estimate the position and attitude of the target object P, the estimation accuracy can be improved. Furthermore, by using the individual reliability of each sensor and the distance from the sensor to the estimated position of the target P, the reliability can be calculated appropriately, thereby allowing for proper control of the accuracy of the estimation results.

[0177] Furthermore, the automatic movement of the mobile body 10 of the present invention includes: a detection control unit 74, which causes a sensor 26 provided on the mobile body 10 to detect a target object P and obtains the detection result of the sensor 26 as a point group M0; and a reliability information acquisition unit 78, which obtains reliability information regarding the accuracy of the estimation result of the position and attitude of the target object P based on the point group M0. The position and attitude of the target object P are estimated based on the point group M0, and based on the estimation result of the position and attitude of the target object P, the position of the mobile body 10 at the time of detection, and the performance of the sensor 26, an ideal number of point groups that can be obtained by the detection of the sensor 26 is calculated. Reliability is then calculated based on a comparison between the ideal number of point groups and the actual number of point groups obtained. By using the reliability calculated in this way, the mobile body 10 can appropriately move to the target position.

[0178] Furthermore, the program of the present invention enables a computer to execute a control method for automatically moving a mobile body 10. This program causes the computer to perform the following steps: a sensor 26 installed on the mobile body 10 detects a target object P, and the detection result of the sensor 26 is obtained as a point group M0; the position and orientation of the target object P are estimated based on the point group M0; based on the estimated position and orientation of the target object P, the position of the mobile body 10 at the time of detection, and the performance of the sensor 26, an ideal number of point groups that can be obtained assuming the detection by the sensor 26 is sufficient is calculated; and based on a comparison between the ideal number of point groups and the actual number of point groups obtained, the reliability of the estimated position and orientation of the target object P in terms of accuracy is calculated. According to this program, the mobile body 10 can be appropriately moved to the target position.

[0179] Furthermore, the control method for the mobile body 10 of the present invention is a control method for an automatically moving mobile body 10, which includes the following steps: causing a sensor 26 installed on the mobile body 10 to detect a target object P multiple times, and obtaining the detection result of each detection by the sensor 26 as a point group M0; estimating the position and orientation of the target object P based on the point group M0 for each detection by the sensor 26; calculating the reliability of the estimation result for the position and orientation of the target object P in terms of accuracy for each detection by the sensor 26; selecting an estimated result for setting a second path R2 (approach path) to a target position relative to the target object P that is in a predetermined position and orientation based on the estimated result of each detection by the sensor 26 based on the reliability of each detection; setting the second path R2 based on the selected estimated result; and moving the mobile body 10 along the second path R2. According to this control method, the reliability of each estimated result is calculated, and the estimated result for setting the second path R2 is selected based on the reliability, so that the mobile body 10 can be appropriately moved to the target position using a highly accurate estimated result.

[0180] Furthermore, in the step of estimating the position and attitude of the target object P, the position and attitude of the target object P are estimated based on the first point group corresponding to the first detection of sensor 26, and simultaneously based on the second point group corresponding to the second detection of sensor 26. Then, in the reliability calculation step, the reliability of the estimation result based on the first point group and the reliability of the estimation result based on the second point group are calculated. In the step of selecting the estimation result, the estimation result with higher reliability is selected from the estimation results based on the first point group and the estimation results based on the second point group. According to this control method, the moving body 10 can be appropriately moved to the target position using a highly accurate estimation result.

[0181] Furthermore, in the step of selecting the estimated result, the estimated result with the highest reliability is selected from each estimated result detected by sensor 26. According to this control method, the moving body 10 can be appropriately moved to the target position using a highly accurate estimated result.

[0182] Furthermore, in the step of selecting the estimated result, the estimated result is also selected based on the detection distance from the position of the moving body 10 when the detection was performed to the estimated position of the target object P. According to this control method, the detection distance is also used to select the estimated result for setting the second path R2, so a more accurate estimated result can be selected, and the moving body 10 can be moved appropriately to the target position.

[0183] Furthermore, in the step of selecting the estimated result, the estimated result with a reliability of above the threshold and the smallest detection distance among the estimated results with a reliability of above the threshold is selected. According to this control method, a more accurate estimated result can be selected, enabling the moving body 10 to move appropriately to the target position.

[0184] Furthermore, the control method of the present invention further includes the following steps: obtaining information on a wide-area path (first path R1) that traverses the setting area AR0 where the target object P is set; and moving the moving body 10 along the first path R1. In the step of obtaining the point group M0, the sensor 26 detects the target object P multiple times during the movement of the moving body 10 along the first path R1. According to this control method, by performing multiple detections by the sensor 26 during the movement traversing the target object P, the target object P can be detected appropriately, and the reduction in the detection accuracy of the target object P can be appropriately suppressed.

[0185] Furthermore, the automatic movement of the mobile body 10 of the present invention includes: a detection control unit 74, which causes the sensor 26 provided on the mobile body 10 to detect the target object P multiple times and obtains the detection result of each detection by the sensor 26 as a point group M0; a second path information acquisition unit 82 (proximity path information acquisition unit), which acquires information of a second path R2 set based on the point group M0 up to a target position with a predetermined position and attitude relative to the target object P; and a movement control unit 72, which causes the mobile body 10 to move along the second path R2. The second path R2 is set as follows: based on the point group M0, the position and attitude of the target object P are estimated for each detection by the sensor 26; for each detection by the sensor 26, the reliability of the estimation result of the position and attitude of the target object P in terms of accuracy is calculated; based on the reliability of each detection by the sensor 26, an estimation result for setting the second path R2 is selected from the estimation results of each detection by the sensor 26; and the second path R2 is set based on the selected estimation result. The moving body 10 moves along a second path R2 set according to the presumed information selected based on reliability, and thus can move appropriately to the destination position.

[0186] Furthermore, the program of the present invention enables a computer to execute a control method for an automatically moving body 10. This program causes the computer to perform the following steps: multiple detections of a target object P by a sensor 26 installed on the moving body 10, obtaining the detection result of each detection by the sensor 26 as a point group M0; estimating the position and orientation of the target object P based on the point group M0 for each detection by the sensor 26; calculating the reliability of the estimation result for the position and orientation of the target object P in terms of accuracy for each detection by the sensor 26; selecting, from the estimation results of each detection by the sensor 26, an estimated result for setting a second path R2 (approach path) to a target position relative to the target object P that achieves a predetermined position and orientation, based on the reliability of each detection by the sensor 26; setting the second path R2 based on the selected estimation result; and moving the moving body 10 along the second path R2. According to this program, a highly accurate estimation result can be used to appropriately move the moving body 10 to the target position.

[0187] The embodiments of the present invention have been described above, but the embodiments are not limited to the content of these embodiments. Furthermore, the aforementioned constituent elements include elements of substantially the same, so-called equivalent scope, which can be easily conceived by those skilled in the art. Moreover, the aforementioned constituent elements can be appropriately combined. Furthermore, various omissions, substitutions, or modifications of the constituent elements can be made without departing from the spirit of the aforementioned embodiments.

Claims

1. A method for controlling a moving body, wherein the moving body moves automatically, The control method for the moving body includes the following steps: The sensor installed on the moving body detects the target object multiple times, and the detection result of each detection by the sensor is obtained as a point group; Based on the point group, the position and orientation of the target object are estimated for each detection by the sensor; For each detection by the sensor, the reliability of the estimation result regarding the position and attitude of the target object is calculated in terms of accuracy. Based on the reliability of each detection by the sensor, from the estimated results of each detection by the sensor, an estimated result for setting an approach path to a target position relative to the target object with a specified position and attitude is selected; The approach path is set based on the selected estimation result; as well as The moving body is moved along the approach path. The step of estimating the position and orientation of the target object includes the following steps: Multiple straight lines connecting two points randomly selected from the obtained point group are used as approximate line candidates. For each of the plurality of approximate line candidates, a cumulative score is calculated, the cumulative score being calculated based on the perpendicular distance between each point constituting the point group and the approximate line candidate; Based on the cumulative score, an approximate line is selected from a plurality of approximate line candidates; and The position and orientation of the target object are estimated based on the approximate line.

2. The control method for a moving body according to claim 1, wherein, In the step of estimating the position and orientation of the target object, the position and orientation of the target object are estimated based on the first point group corresponding to the first detection of the sensor, and simultaneously based on the second point group corresponding to the second detection of the sensor. In the step of calculating the reliability, the reliability based on the inference result of the first point group and the reliability based on the inference result of the second point group are calculated. In the step of selecting the presumption result, the presumption result with higher reliability is selected from the presumption results based on the first point group and the presumption results based on the second point group.

3. The control method for a moving body according to claim 2, wherein, In the step of selecting the estimated result, among the estimated results of each detection by the sensor, the estimated result with the highest reliability is selected.

4. The control method for a moving body according to claim 1 or 2, wherein, In the step of selecting the estimated result, the estimated result is also selected based on the distance from the position of the moving body when the detection was performed to the estimated position of the target object.

5. The control method for a moving body according to claim 4, wherein, In the step of selecting the presumed result, the presumed result whose reliability is above the threshold is selected, and whose distance is the smallest among the presumed results whose reliability is above the threshold.

6. The control method for a moving body according to claim 1, wherein, The control method for the moving body further includes the following steps: Obtain information on the wide-area path that traverses the setting area of ​​the target object; and The moving body is moved along the wide-area path. In the step of obtaining the detection results of each detection by the sensor as a point group, the sensor detects the target object multiple times as the moving body moves along the wide-area path.

7. A mobile body that moves automatically, wherein, The mobile body includes: The detection control unit causes the sensors installed on the moving body to detect the target object multiple times, and obtains the detection results of each detection by the sensors as a point group; The approach path information acquisition unit acquires information about the approach path based on the point group up to a target position relative to the target object, where the target object has a predetermined position and attitude; and A movement control unit that moves the moving body along the approach path. The proximity path is set as follows: Based on the point group, the position and orientation of the target object are estimated for each detection by the sensor; For each detection by the sensor, the reliability of the estimation result regarding the position and attitude of the target object is calculated in terms of accuracy. Based on the reliability of each detection by the sensor, a prediction result for setting the proximity path is selected from the prediction results of each detection by the sensor; and The approach path is set based on the selected estimation result. The position and orientation of the target object are presumed as follows: Multiple straight lines connecting two points randomly selected from the obtained point group are used as approximate line candidates. For each of the plurality of approximate line candidates, a cumulative score is calculated, the cumulative score being calculated using the perpendicular distances between each point constituting the point group and the approximate line candidate; Based on the cumulative score, an approximate line is selected from a plurality of approximate line candidates; and The position and orientation of the target object are estimated based on the approximate line.

8. A storage medium that stores a computer-readable storage medium having a program storing a control method for causing a computer to execute automatic movement of a moving body, wherein, The procedure includes the following steps: The sensor installed on the moving body detects the target object multiple times, and the detection result of each detection by the sensor is obtained as a point group; Based on the point group, the position and orientation of the target object are estimated for each detection by the sensor; For each detection by the sensor, the reliability of the estimation result regarding the position and attitude of the target object is calculated in terms of accuracy. Based on the reliability of each detection by the sensor, from the estimated results of each detection by the sensor, an estimated result for setting an approach path to a target position relative to the target object with a specified position and attitude is selected; The approach path is set based on the selected estimation result; as well as The moving body is moved along the approach path. The step of estimating the position and orientation of the target object includes the following steps: Multiple straight lines connecting two points randomly selected from the obtained point group are used as approximate line candidates. For each of the plurality of approximate line candidates, a cumulative score is calculated, the cumulative score being calculated using the perpendicular distances between each point constituting the point group and the approximate line candidate; Based on the cumulative score, an approximate line is selected from a plurality of approximate line candidates; and The position and orientation of the target object are estimated based on the approximate line.

Images