JP7482808B2 - MOBILE BODY CONTROL METHOD, MOBILE BODY, AND PROGRAM - Google Patents

Description

This disclosure relates to a method for controlling a moving object, a moving object, and a program.

Automatically moving mobile objects equipped with sensors that detect the surroundings are known. One such mobile object is an automated forklift that transports pallets loaded with cargo. Patent Document 1 describes how the center position and direction of the front of the pallet are identified based on distance data measured by a range sensor, and the cargo is removed from the pallet.

JP 2017-178567 A

In such mobile objects, there is a demand for improved detection accuracy for objects such as pallets.

The present disclosure aims to solve the above-mentioned problems and provide a method for controlling a moving object, a moving object, and a program that can suppress a decrease in the accuracy of detecting an object.

In order to solve the above-mentioned problems and achieve the object, the control method of a moving body according to the present disclosure is a control method of a moving body that moves automatically, and includes the steps of: detecting a target object multiple times by irradiating a sensor provided on the moving body with light, and acquiring the results of the multiple detections by the sensor as a point cloud; detecting an approximation line of the point cloud based on a first point cloud that is a point cloud corresponding to the first detection of the sensor; calculating an angle of incidence of light from the sensor to the target object in the second detection based on the approximation line, a second point cloud that is a point cloud corresponding to the second detection of the sensor, and the position of the moving body when the second detection is performed; selecting a point cloud from the second point cloud based on the angle of incidence to identify the position and orientation of the target object; and identifying the position and orientation of the target object based on the point cloud selected from the second point cloud.

In order to solve the above-mentioned problems and achieve the object, the moving body according to the present disclosure is an automatically moving moving body, and includes a detection control unit that detects a target multiple times by irradiating a sensor provided on the moving body with light, and acquires the results of the multiple detections of the sensor as a point cloud, and a target information acquisition unit that acquires information on the position and attitude of the target identified based on the point cloud, and the position and attitude of the target are identified based on the point cloud by calculating an incident angle of light from the sensor to the target in the second detection based on an approximation line of the point cloud detected based on a first point cloud that is a point cloud corresponding to a first detection of the sensor, a second point cloud that is a point cloud corresponding to a second detection of the sensor, and the position of the moving body when the second detection is performed, and a point cloud for identifying the position and attitude of the target is selected from the second point cloud based on the incident angle, and the position and attitude of the target are identified based on the point cloud selected from the second point cloud.

In order to solve the above-mentioned problems and achieve the object, the program according to the present disclosure is a program for causing a computer to execute a method for controlling an automatically moving object, and causes the computer to execute the following steps: detecting a target object multiple times by irradiating a sensor provided on the moving object with light, and acquiring the results of the multiple detections by the sensor as a point cloud; detecting an approximation line of the point cloud based on a first point cloud that is a point cloud corresponding to the first detection of the sensor; calculating an angle of incidence of light from the sensor to the target object in the second detection based on the approximation line, a second point cloud that is a point cloud corresponding to the second detection of the sensor, and the position of the moving object when the second detection is performed; selecting a point cloud from the second point cloud based on the angle of incidence to identify the position and orientation of the target object; and identifying the position and orientation of the target object based on the point cloud selected from the second point cloud.

According to the present disclosure, it is possible to suppress a decrease in the detection accuracy of the target object.

FIG. 1 is a schematic diagram of a mobility control system according to the first embodiment. FIG. 2 is a schematic diagram of the configuration of a moving body. FIG. 3 is a schematic block diagram of the management system. FIG. 4 is a schematic block diagram of an information processing device. FIG. 5 is a schematic block diagram of a control device for a moving object. FIG. 6 is a schematic diagram for explaining detection of a target object. FIG. 7 is a schematic diagram for explaining the selection of a group of points. FIG. 8 is a schematic diagram for explaining extraction of a point cloud. FIG. 9 is a schematic diagram showing a specific example of a line candidate. FIG. 10 is a graph showing an example of the scores. FIG. 11 is a schematic diagram illustrating an example of detection of a straight line. FIG. 12 is a schematic diagram for explaining a specific example of superimposition of point clouds. FIG. 13 is a schematic diagram showing an example of an approximation line. FIG. 14 is a schematic diagram showing an example of measurement points projected onto an approximation line. FIG. 15 is a schematic diagram showing an example of a histogram of measurement points. FIG. 16 is a schematic diagram illustrating an example of a method for identifying the position of a target object. FIG. 17 is a schematic diagram showing an example of movement along the second path. FIG. 18 is a flowchart illustrating a process flow for detecting the position and orientation of a target object. FIG. 19 is a schematic diagram illustrating an example of a method for identifying the position of a target object in the first modified example. FIG. 20 is a schematic diagram illustrating an example of a method for identifying the position of a target object in the second modified example.

The following describes in detail a preferred embodiment of the present invention with reference to the attached drawings. Note that the present invention is not limited to this embodiment, and when there are multiple embodiments, the present invention also includes a configuration in which each embodiment is combined.

First Embodiment
(Overall configuration of the mobility control system)
FIG. 1 is a schematic diagram of a movement control system according to a first embodiment. As shown in FIG. 1, the movement control system 1 according to 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 that controls the movement of a moving body 10 belonging to a facility W. The facility W is a facility that is managed by logistics, such as a warehouse. In the movement control system 1, a target object P arranged in an area AR of the facility W is picked up and transported by the moving body 10. The area AR is, for example, the floor surface of the facility W, and is an area in which the target object P is installed and the moving body 10 moves. In this embodiment, the target object P is a transport target object in which luggage is loaded on a pallet. The target object P has a front surface Pa formed with a plurality of pillars PA and an opening PB formed between the pillars PA. The front surface Pa refers to the surface on the side from which the moving body 10 approaches. The moving body 10 holds the target object P by inserting a fork 24, which will be described later, into the opening PB. However, the target P is not limited to a pallet with luggage loaded on it, and may be in any form, for example, it may be luggage only without a pallet. Hereinafter, one direction along the area AR is referred to as direction X, and a direction along the area A that intersects with direction X is referred to as direction Y. In this embodiment, direction Y is a direction perpendicular to direction X. Directions X and Y may also be referred to as horizontal directions. Moreover, a direction perpendicular to directions X and Y, i.e., the vertical direction, is referred to as direction Z.

A plurality of installation areas AR0 are provided in the area AR in the equipment W. The installation area AR0 is an area in which the target object P is to be installed. The installation area AR0 is set in advance as an area in which the target object P should be installed. The installation area AR0 is divided, for example, by a white line, and the position (coordinates), shape, and size of the installation area AR0 are set in advance. In the installation area AR0, the target object P is arranged so that the front surface Pa faces the direction X side. In the example of FIG. 1, the target object P is arranged in the installation area AR so that the axis PX perpendicular to the front surface Pa when viewed from the direction Z is along the direction X, that is, so that the orientation of the target object P does not deviate from the installation area AR0. However, the axis PX of the target object P is not limited to being along the direction X, and the axis PX may be installed at an angle from the direction X, that is, so that the orientation of the axis PX is deviated from the installation area AR. For example, it is preferable that the target object P is arranged in the installation area AR0 so that the inclination angle between the axis PX and the direction X is 45 degrees or less.

In this embodiment, the installation area AR0 is provided in the area AR, which is the floor of the facility W, but is not limited thereto. For example, the installation area AR0 may be provided in the loading platform of a vehicle that has brought the target object P into the facility W. In this embodiment, the installation area AR0 is divided into sections for each target object P, and one target object P is placed in the installation area AR0, but is not limited thereto. For example, the installation area AR0 may be set as a free space in which multiple targets P can be placed. In the example of FIG. 1, the installation area AR0 is rectangular, but the shape and size may be arbitrary. The number of installation areas AR0 provided in the area AR may also be arbitrary.

The moving body 10 is a device that can move automatically. In this embodiment, the moving body 10 is a forklift, or more specifically, a so-called AGF (Automated Guided Forklift). As illustrated in FIG. 1, the moving body 10 moves on an area AR in the facility W. The moving body 10 detects the target P multiple times by a sensor 26 described later while moving from a first position A1 to a second position A2 according to a first path R1 (wide area path). The moving body 10 acquires a point cloud from multiple detection results by the sensor 26, and identifies the position and posture of the target P based on the point cloud. When the moving body 10 reaches the second position A2, it moves from the second position A2 to the target position A3 according to a second path R2 (approach path) set based on the position and posture of the target P, and picks up the target P. The position of the target P here refers to the coordinates of the target P in a two-dimensional coordinate system CO in the direction X and the direction Y, and the attitude of the target P refers to the orientation (rotation angle) of the target P when viewed from a direction perpendicular to the direction X and the direction Y. The target position A3 is a position and attitude that is a predetermined position and attitude with respect to the target P. In this embodiment, the target position A3 can be said to be a position and attitude at which the moving body 10 can pick up the target P. For example, the target position A3 may be a position and attitude of the moving body 10 at which the fork 24 of the moving body 10 described later can be inserted into the opening PB of the target P by moving straight without moving laterally. In this case, the moving body 10 moves straight from the target position A3 to pick up the target P and transport the target P to another location. A specific method for identifying the position and attitude of the target P and details of the movement according to the first pass R1 and the second pass R2 will be described later. The first pass R1 and the second pass R2 shown in FIG. 1 are examples.

(Mobile)
FIG. 2 is a schematic diagram of the configuration of the moving body. As shown in FIG. 2, the moving body 10 includes a vehicle body 20, a mast 22, a fork 24, a sensor 26, and a control device 28. The vehicle body 20 includes a wheel 20A. The mast 22 is provided at one end of the vehicle body 20 in the front-rear direction. The mast 22 extends along a vertical direction (here, direction Z) perpendicular to the front-rear direction. The fork 24 is attached to the mast 22 so as to be movable in the direction Z. The fork 24 may also be movable in the lateral direction of the vehicle body 20 (a direction intersecting the vertical direction and the front-rear direction) relative to the mast 22. The fork 24 has a pair of claws 24A, 24B. The claws 24A, 24B extend from the mast 22 toward the front of the vehicle body 20. The claws 24A and 24B are arranged apart from each other in the lateral direction of the mast 22. Hereinafter, of the front-rear direction, the direction on the side of the movable body 10 on which the fork 24 is provided is referred to as the front direction, and the direction on the side on which the fork 24 is not provided is referred to as the rear direction.

The sensor 26 detects at least one of the position and the attitude of an object present around the vehicle body 20. It can also be said that the sensor 26 detects the position of the object relative to the moving body 10 and the attitude of the object relative to the moving body 10. In this embodiment, the sensor 26 is provided on the mast 22 and on the four corners of the vehicle body 20, i.e., on the left and right ends on the forward side and the left and right ends on the rear side of the vehicle body 20. However, the position at which the sensor 26 is provided is not limited to this, and the sensor 26 may be provided at any position, and the number of sensors provided may also be arbitrary. For example, a safety sensor provided on the moving body 10 may be used as the sensor 26. By reusing the safety sensor, there is no need to provide a new sensor.

The sensor 26 detects the position and orientation of the object by detecting (receiving) reflected light from the surrounding object. More specifically, the sensor 26 is a sensor that irradiates light, more specifically, it irradiates laser light as light. The sensor 26 detects the position and orientation of the object by detecting the reflected light of the irradiated laser light. The sensor 26 irradiates laser light while scanning in one direction, and detects the position and orientation of the object from the reflected light of the irradiated laser light. In other words, the sensor 26 can be said to be a so-called 2D-LiDAR (Light Detection and Ranging). In this embodiment, the sensor 26 scans the laser light in the horizontal direction, that is, in a direction perpendicular to the direction Z. However, the sensor 26 is not limited to the above and may be a sensor that detects the object by any method, for example, a so-called 3D-LiDAR that scans in multiple directions.

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

(Management System)
FIG. 3 is a schematic block diagram of the management system. The management system 12 is a system that manages logistics in the facility W. In this embodiment, the management system 12 is a WMS (warehouse management system), but is not limited to a WMS and may be any system, for example, a back-end system such as other production management systems. The location where the management system 12 is installed is arbitrary, and the system may be installed within the facility W, or may be installed at a location away from the facility W to manage the facility W from the remote location. The management system 12 is a computer, and includes a communication unit 30, a storage unit 32, and a control unit 34 as shown in FIG. 3.

The control unit 34 is a calculation device, i.e., a CPU (Central Processing Unit). The control unit 34 includes a work determination unit 36. The control unit 34 realizes the work determination unit 36 and executes the processing by reading and executing a program (software) from the memory unit 32. Note that the control unit 34 may execute the processing by one CPU, or may be provided with multiple CPUs and execute the processing by the multiple CPUs. The work determination unit 36 may also be realized by a hardware circuit. Furthermore, the program for the control unit 34 saved in the memory unit 32 may be stored in a recording medium that can be read by the management system 12.

The work determination unit 36 determines the target object P to be transported. Specifically, the work determination unit 36 determines the work content indicating the information of the target object P to be transported, for example, based on an input work plan. The work content can also be said to be information that specifies the target object P to be transported. In the example of this embodiment, the work content determines which target object P in which facility is to be transported, by when, and to where. In other words, the work determination unit 36 determines the information indicating the facility in which the target object P is stored, the target object P, the destination of the target object P, and the transport time of the target object P. The work determination unit 36 transmits the determined work content to the information processing device 14 via the communication unit 30.

(Information processing device)
FIG. 4 is a schematic block diagram of an information processing device. The information processing device 14 is provided in the facility W, and is a device that transmits and receives at least information related to the movement of the mobile body 10 to and from the mobile body 10, that is, a so-called ground system. The information processing device 14 is a computer, and includes a communication unit 40, a storage unit 42, and a control unit 44, as shown in FIG. 4. The communication unit 40 is a module used by the control unit 44 to communicate with external devices such as the management system 12 and the mobile body 10, and may include, for example, an antenna. In this embodiment, the communication method by the communication unit 40 is wireless communication, but the communication method may be arbitrary. The storage unit 42 is a memory that stores various information such as the calculation contents and programs of the control unit 44, and includes, for example, at least one of a RAM, a main storage device such as a ROM, and an external storage device such as an HDD.

The control unit 44 is a calculation device, that is, a CPU. The control unit 44 includes a work content acquisition unit 50, a moving object selection unit 52, and a first path acquisition unit 54. The control unit 44 realizes the work content acquisition unit 50, the moving object selection unit 52, and the first path acquisition unit 54 by reading and executing a program (software) from the storage unit 42, and executes the processes. The control unit 44 may execute these processes using one CPU, or may be provided with multiple CPUs and execute the processes using the multiple CPUs. In addition, at least a part of the work content acquisition unit 50, the moving object selection unit 52, and the first path acquisition unit 54 may be realized by a hardware circuit. In addition, the program for the control unit 44 stored in the storage unit 42 may be stored in a recording medium that can be read by the information processing device 14.

(Work content acquisition unit and moving object selection unit)
The work content acquisition unit 50 acquires information on the work content determined by the management system 12, that is, information on the target object P to be transported. The work content acquisition unit 50 identifies the installation area AR0 in which the target object P is installed from the information on the target object P in the work content. For example, the storage unit 42 stores the target object P and the installation area AR0 in which the target object P is installed in association with each other, and the work content acquisition unit 50 identifies the installation area AR0 by reading out the information from the storage unit 42. The moving body selection unit 52 selects the target moving body 10. For example, the moving body selection unit 52 selects the target moving body 10 from a plurality of moving bodies belonging to the facility W. The moving body selection unit 52 may select the target moving body 10 by any method, but may select, for example, a moving body 10 suitable for transporting the target object P in the installation area AR0 based on the installation area AR0 identified by the work content acquisition unit 50 as the target moving body 10.

(First path acquisition unit)
The first path acquisition unit 54 acquires information on the first path R1 to the installation area AR0 identified by the work content acquisition unit 50. The first path R1 is set in advance for each installation area AR0, for example. The first path acquisition unit 54 acquires the first path R1 set for the installation area AR0 identified by the work content acquisition unit 50, for example, from the storage unit 42. The first path R1 will be specifically described below.

As shown in FIG. 1, the first path R1 is a trajectory that crosses the installation area AR0 (target P) along the direction Y on the direction X side of the installation area AR0 (target P) toward which the moving body 10 is heading. More specifically, the first path R1 includes a detection trajectory R1a and an approach trajectory R1b that is connected to the detection trajectory R1a.

1, the detection trajectory R1a is a trajectory that crosses the installation area AR0 (target P) along the direction Y on the direction X side of the installation area AR0 (target P). It is preferable that the detection trajectory R1a is set so that the distance in the direction X to the installation area AR0 is within a range of a predetermined distance. The predetermined distance here is a distance at which the position and posture of the target P in the installation area AR0 can be detected by the sensor 26 of the moving body 10 moving along the detection trajectory R1a. More specifically, in this embodiment, the detection trajectory R1a is a trajectory from the first position A1 to the second position A2. The first position A1 is set as a position on the direction X side of the installation area AR0 and on the opposite side of the direction Y from the installation area AR0. The second position A2 is set as a position on the direction X side of the installation area AR0 and on the direction Y side of the installation area AR0. In this embodiment, the first position A1 and the second position A2 are set so that the distance in the direction X to the installation area AR0 is within a predetermined distance range, and the positions in the direction X (X coordinates) of the first position A1 and the second position A2 are the same. The detection trajectory R1a is set as a straight trajectory along the direction Y from the first position A1 to the second position A2. However, the positions in the direction X (X coordinates) of the first position A1 and the second position A2 do not have to be the same. In addition, the detection trajectory R1a is not limited to being a straight trajectory, and may be a trajectory that draws an arbitrary locus from the first position A1 to the second position A2.

As shown in FIG. 1, the approach trajectory R1b is a trajectory from the second position A2 toward the installation area AR0. More specifically, the approach trajectory R1b is a trajectory from the second position A2 to the set position A3z. The set position A3z is a position and attitude that is a predetermined position and attitude for the target P, assuming that the position and attitude of the target P in the installation area AR0 satisfy a predetermined state (the target P is ideally placed without deviation in the installation area AR0). In other words, the set position A3z is a position and attitude at which the moving body 10 can pick up the target P, assuming that the position and attitude of the target P satisfy a predetermined state, and can also be said to be the target position A3 when the position and attitude of the target P satisfy a predetermined state. In the example of FIG. 1, the approach trajectory R1b includes a straight trajectory from the second position A2 to the intermediate position ASB0 on the opposite side of the second position A2 in the direction Y, and a curved trajectory from the intermediate position ASB0 to the set position A3z. It is preferable that the straight line trajectory from the second position A2 to the intermediate position ASB0 overlaps with the detection trajectory R1a.

Although not shown in the figure, the first path R1 may also include the trajectory from the movement start position of the moving body 10 to the first position A1.

However, the first path R1 is not limited to the above-mentioned trajectory. For example, the first path R1 may not include the approach trajectory R1b. That is, the first path R1 may include at least the detection trajectory R1a from the first position A1 to the second position A2. Furthermore, the first path R1 is not limited to including a trajectory that crosses the installation area AR0 (target P) like the detection trajectory R1a, and may be, for example, a trajectory that approaches the front of the installation area AR0 (target P) in a straight line. Furthermore, the first path R1 is not limited to the moving body 10 moving straight backward, crossing the installation area AR0 (target P) and turning back at the second position A2, but may be a trajectory in which the moving body 10 moves straight forward and turns to approach the front of the installation area AR0 (target P).

The first path R1 is set in advance based on the map information of the facility W. The map information of the facility W includes position information of obstacles (such as pillars) installed in the facility W and paths through which the mobile body 10 can travel, and can be said to be information indicating the area in which the mobile body 10 can move within the area AR. The first path R1 may be set based on the vehicle specification information of the mobile body 10 in addition to the map information of the facility W. The vehicle specification information is, for example, specifications that affect the route through which the mobile body 10 can move, such as the size and minimum turning radius of the mobile body 10. When the first path R1 is set based on the vehicle specification information as well, the first path R1 may be set for each mobile body. The first path R1 may be set by a person based on map information, vehicle specification information, etc., or may be automatically set by a device such as the information processing device 14 based on map information, vehicle specification information, etc. When automatically setting the first path R1, for example, a point (waypoint) that you want to pass through can be specified. In this case, it is possible to set the first path R1 that passes through the desired point, is the shortest, and avoids obstacles (fixed objects such as walls).

The first path acquisition unit 54 may set the first path R1 by calculation, without reading out the preset first path R1. In this case, the first path acquisition unit 54 may generate, as the first path R1, a route from the current position of the mobile body 10 to the destination setting position A3z, via the first position A1 and the second position A2, based on the position information of the target mobile body 10, the position information of the installation area AR0, and the map information of the equipment W.

The information processing device 14 transmits the acquired information of the first path R1 to the target moving body 10 via the communication unit 40. Since the first path R1 is a route toward the installation area AR0, it can be said to be information regarding the movement of the moving body 10.

(Control device for mobile object)
Next, the control device 28 of the moving body 10 will be described. FIG. 5 is a schematic block diagram of the control device of the moving body. The control device 28 controls the moving body 10. The control device 28 moves the moving body 10 to the target position A3 along the second path R2 set based on the detection results of the sensor 26 of the moving body 10 multiple times, and causes the moving body 10 to pick up the target P. The control device 28 is a computer, and includes a communication unit 60, a storage unit 62, and a control unit 64 as shown in FIG. 5. The communication unit 60 is a module used by the control unit 64 to communicate with an external device such as the information processing device 14, and may include, for example, an antenna. The communication method by the communication unit 40 is wireless communication in this embodiment, but the communication method may be arbitrary. The storage unit 62 is a memory that stores various information such as the calculation contents and programs of the control unit 64, and includes, for example, at least one of a RAM, a main storage device such as a ROM, and an external storage device such as an HDD.

The control unit 64 is a calculation device, that is, 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 point cloud superimposition unit 76, a target information acquisition unit 78, and a second path information acquisition unit 80. The control unit 64 reads out and executes a program (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 point cloud superimposition unit 76, the target information acquisition unit 78, and the second path information acquisition unit 80, and executes the processes. The control unit 64 may execute these processes by one CPU, or may be provided with multiple CPUs and execute the processes by the multiple CPUs. In addition, at least a part of the first path information acquisition unit 70, the movement control unit 72, the detection control unit 74, the point cloud superimposition unit 76, the target information acquisition unit 78, and the second path information acquisition unit 80 may be realized by a hardware circuit. In addition, the program for the control unit 64 stored in the memory unit 62 may be stored in a recording medium that can be read by the control device 28.

(First path information acquisition unit)
The first path information acquisition unit 70 acquires information on the first path R1. When the moving body 10 is selected as a work target, the first path information acquisition unit 70 may acquire the information on the first path R1 from the information processing device 14, or may read out the information on the first path R1 stored in advance in the storage unit 62.

(Movement control unit)
The movement control unit 72 controls the movement of the moving body 10 by controlling the movement mechanism such as the drive unit and steering of the moving body 10. The movement control unit 72 moves the moving body 10 according to the first path R1 acquired by the first path information acquisition unit 70 and the second path R2 acquired by the second path information acquisition unit 80 at the subsequent stage. The movement control unit 72 moves the moving body 10 so as to pass through the first path R1 and the second path R2 by sequentially grasping the position information of the moving body 10. The method of acquiring the position information of the moving body 10 is arbitrary, but for example, in this embodiment, a detection body (not shown) is provided in the facility W, and the movement control unit 72 acquires information on the position and attitude of the moving body 10 based on the detection of the detection body. Specifically, the moving body 10 irradiates a laser beam toward the detection body and receives the reflected light of the laser beam by the detection body to detect its own position and attitude in the facility W. The position of the moving body 10 here refers to two-dimensional coordinates in the direction X and direction Y in the area A of the facility W, and hereinafter, unless otherwise specified, the position refers to the two-dimensional coordinates in the area A. Furthermore, the attitude of the moving body 10 refers to the orientation (rotation angle) of the moving body 10 when viewed from the direction Z perpendicular to the direction X and direction Y. Furthermore, the method of acquiring information on the position and attitude of the moving body 10 is not limited to using a detection object, and for example, a self-location estimation technology such as SLAM (Slmutaneous Localization and Mapping) may be used.

(Detection control unit)
The detection control unit 74 causes the sensor 26 to detect the front surface Pa of the target P multiple times, and acquires the multiple detection results of the sensor 26 as a point cloud. The detection control unit 74 also calculates the angle of incidence of the light irradiated from the sensor 26 to the target P, and selects a point cloud for identifying the position and orientation of the target P from among the acquired point clouds based on the angle of incidence. Specific processing contents of the detection control unit 74 will be described later.

(Point cloud superposition part)
The point cloud superimposing unit 76 superimposes, in the same coordinate system, point clouds corresponding to the multiple detection results of the sensor 26 acquired by the detection control unit 74. Specific processing by the point cloud superimposing unit 76 will be described later.

(Target position information acquisition unit)
The target information acquisition unit 78 acquires the result of identifying the position and orientation of the target P, which is identified based on the position of the point cloud selected by the detection control unit 74. More specifically, the target information acquisition unit 78 of this embodiment acquires the result of identifying the position and orientation of the target P, which is identified based on the position of the superimposed point cloud. Specific processing by the target information acquisition unit 78 will be described later.

(Second path information acquisition unit)
The second path information acquisition unit 80 acquires information on a second path R2 to a target position A3, which is set based on the position and orientation of the target. Specific processing by the second path information acquisition unit 80 will be described later.

(Control device processing)
Next, the process of the control device 28 when the moving body 10 moves toward the target P will be described.

(Movement along the first path)
The movement control unit 72 of the control device 28 moves the moving object 10 according to the first path R1 acquired by the first path information acquisition unit 70. The movement control unit 72 moves the moving object 10 from the current position of the moving object 10 to the second position A2 via the first position A1, so as to pass through the first path R1.

(Point cloud acquisition)
6 is a schematic diagram for explaining detection of a target. As shown in FIG. 6, while the moving body 10 is moving between the first position A1 and the second position A2 along the first path R1, that is, while moving along the detection track R1a, the detection control unit 74 causes the sensor 26 to detect the front surface Pa of the target P multiple times. The detection control unit 74 causes the sensor 26 to detect the front surface Pa of the target P by performing detection toward the installation area AR0 whose position is known. Specifically, the detection control unit 74 causes the sensor 26 to irradiate the laser light LT toward the installation area AR in which the target P is installed. The laser light LT is reflected by the front surface Pa of the target P, and the sensor 26 receives the reflected light from the front surface Pa of the target P. The detection control unit 74 acquires a point cloud M0, which is a set of measurement points M, based on the detection result of the reflected light received by the sensor 26. The measurement point M is a point indicating the position (coordinate) at which the laser light LT is reflected, and the point group M0 refers to a collection of points indicating the positions at which the laser light LT is reflected. In this embodiment, the detection control unit 74 calculates the position (coordinate) of the location at which the reflected light is reflected in a two-dimensional coordinate system CO in the direction X and the direction Y 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 determining the position in the two-dimensional coordinate system CO as the measurement point M, and may determine the position in a coordinate system based on the sensor 26 or the moving body 10 as the measurement point M.

(Selection of points)
7 is a schematic diagram for explaining the selection of the point cloud. The detection control unit 74 estimates the incident angle θ of the light irradiated from the sensor 26 to the target P during detection, and selects the measurement points M to be used for identifying the position and orientation of the target P from among the measurement points M included in the point cloud M0 acquired by the detection based on the estimated incident angle θ. An example of the process of selecting the measurement points M based on the incident angle θ will be specifically explained below.

(Calculation of Approximation Line K)
In order to calculate the incident angle θ, the detection control unit 74 calculates an approximation line K for the measurement point M acquired by a detection (scan) other than the current detection (scan). In other words, if the detection corresponding to the measurement point M selected this time is the second detection, the detection control unit 74 calculates the approximation line K for the measurement point M acquired by the first detection based on the measurement point M acquired by the first detection, which is a detection other than the second detection. Since the measurement point M acquired by the first detection corresponds to the front surface Pa of the target P, the approximation line K corresponds to the orientation of the front surface Pa of the target P. In this embodiment, the detection control unit 74 calculates the approximation line K by performing straight line fitting for each measurement point M by the least squares method. However, the calculation method of the approximation line K is not limited to the least squares method, and any method may be used. In addition, the detection control unit 74 may calculate the approximation line K using some of the measurement points M acquired by the first detection, or may calculate the approximation line K using all of the measurement points M acquired by the first detection. In this embodiment, since the front surface Pa of the target P is flat, the approximation line K is calculated as a straight line, but the approximation line is not limited to being a straight line. For example, if it is known that the front surface Pa is a curved surface, the approximation line K may be calculated as a curved line accordingly.

In this embodiment, if there is a measurement point M obtained by multiple detections before the current measurement point M is selected, the detection control unit 74 calculates the approximation line K of the measurement points M obtained by the multiple detections. In other words, if the detection corresponding to the measurement point M selected this time is the third detection, and the detection corresponding to the measurement point M obtained before the current measurement point M is detected is the first detection and the second detection, the detection control unit 74 calculates the approximation line K of the measurement points M obtained by the first detection and the second detection based on the measurement points M obtained by the first detection and the second detection. That is, each time the number of detections by the sensor 26 increases, the detection control unit 74 updates the approximation line K by adding the measurement point M obtained by the most recent detection. Note that the detection control unit 74 may calculate the approximation line K using some of the measurement points M obtained by the multiple detections prior to this time, or may calculate the approximation line K using all of them.

More specifically, in this embodiment, the detection control unit 74 detects an approximation line K of the measurement points M superimposed by the point cloud superimposition unit 76 based on the measurement points M superimposed by the point cloud superimposition unit 76 described later. As will be described in detail later, the measurement points M superimposed by the point cloud superimposition unit 76 are measurement points M selected to detect the front surface Pa of the target P with high accuracy, so by using the measurement points M superimposed by the point cloud superimposition unit 76, the approximation line K can be made to correspond more appropriately to the front surface Pa of the target P. However, the detection control unit 74 is not limited to using the measurement points M superimposed by the point cloud superimposition unit 76 to calculate the approximation line K, and may use any measurement points M obtained by a detection other than the current detection.

In order to calculate the incident angle θ, the detection control unit 74 acquires information on the position AR1 of the moving body 10 when the current detection was performed and information on the position of the measurement point M acquired by the current detection. More specifically, the position AR1 is the position in the two-dimensional coordinate system CO of the sensor 26 that performed the current detection when the current detection was performed. As described above, the position of the moving body 10 is detected by the movement control unit 72, and the position of the sensor 26 in the coordinate system of the moving body 10 can be acquired, for example, from design information. Therefore, the detection control unit 74 can acquire information on the position AR1 of the sensor 26 that performed the current detection based on the position of the moving body 10. Note that the position AR1 is not limited to the position of the sensor 26 that performed the current detection, and may be, for example, any reference position of the moving body 10 when the current detection was performed.

(Calculation of the angle of incidence)
The detection control unit 74 calculates the incident angle θ based on the positional relationship between the approximation line K, the position AR1, and the measurement point M acquired by the current detection. The detection control unit 74 calculates the incident angle θ based on the positional relationship between the approximation line K, the position AR1, and the measurement point M by assuming that the direction from the position AR1 toward the measurement point M is the traveling direction of the laser light LT and that the approximation line K is the front surface Pa of the target P. Specifically, as shown in the example of FIG. 7, the detection control unit 74 calculates the angle between a perpendicular line K1 (normal line of the approximation line K) perpendicular to the approximation line K and a straight line K2 connecting the position AR1 and the measurement point M as the incident angle θ. The detection control unit 74 calculates the incident angle θ for each measurement point M acquired by the current detection. However, the calculation method of the incident angle θ is not limited to this. For example, the angle between the approximation line K and the straight line K2 may be calculated as the incident angle θ.

(Selection of points based on incidence angle)
The detection control unit 74 selects a measurement point M for identifying the position and orientation of the target from among the measurement points M acquired by the current detection based on the incident angle θ. The detection control unit 74 saves information on the selected measurement point M in a buffer (temporary storage area) included in the storage unit 62. The detection control unit 74 selects, from among the measurement points M acquired by the current detection, a measurement point M for which the incident angle θ is within a predetermined range as a measurement point M for identifying the position and orientation of the target. On the other hand, the detection control unit 74 does not select a measurement point M for which the incident angle θ is outside the predetermined range as a measurement point M for identifying the position and orientation of the target, and excludes it from the measurement points M for identifying the position and orientation of the target.

The predetermined range may be set arbitrarily, but in this embodiment, it is set in advance so that the detection accuracy of the measurement point M is guaranteed if the incident angle θ is within the predetermined range. In this case, the predetermined range may differ depending on the performance of the sensor 26 and the characteristics of the target P (for example, at least one of the shape, size, material, and color of the target P). Therefore, a sensor of the same type as the sensor 26 used this time may be made to detect a target of the same type as the target P to be detected this time and whose position and orientation are known, and the predetermined range may be set based on the detection result. The predetermined range may also change depending on the distance between the sensor 26 and the target P. In this case, a sensor of the same type as the sensor 26 used this time may be made to detect a target of the same type as the target P to be detected this time and whose position and orientation are known while changing the distance between the target and the sensor, and the predetermined range may be set for each distance between the target and the sensor based on the detection result. Then, the detection control unit 74 may read out the predetermined range corresponding to the distance between the target P and the sensor 26 in the current detection and use it to select the measurement point M. To summarize the above, the predetermined range may be set according to at least one of the performance of the sensor 26, the characteristics of the target P, and the distance between the sensor 26 and the target P. In the present embodiment, the predetermined range is, for example, a range between a predetermined lower limit value and an upper limit value, but is not limited thereto. For example, only the lower limit value may be set without setting the upper limit value. In a type of sensor that detects the target P by irradiating light, if the incident angle θ is small, for example, the intensity of the reflected light of the laser light LT on the target P becomes too high, and the detection accuracy may be significantly reduced. In contrast, by setting the lower limit value in this manner, the measurement point M with low detection accuracy can be excluded, and the decrease in detection accuracy can be appropriately suppressed. In addition, if the incident angle θ is large, for example, the detection accuracy may decrease due to the laser light LT not being well reflected by the target P. Therefore, it is more preferable to also set an upper limit value in the predetermined range.

In this embodiment, since detection is performed each time the moving body 10 moves, the position of the moving body 10 relative to the target P changes with each detection, and the incident angle θ with respect to the same position of the target P also changes with each detection. Therefore, the positions of the measurement points M that are excluded from selection because the incident angle θ falls outside the predetermined range are not concentrated on the front surface Pa of the target P, and it is possible to select the measurement points M of the entire front surface Pa of the target P. This makes it possible to more appropriately suppress a decrease in detection accuracy.

The approximation line K used to select the measurement points M is calculated based on the measurement points M obtained by detection other than the current detection. Therefore, if there are no measurement points M obtained by detection other than the current detection, that is, if there are no measurement points M superimposed by the point cloud superimposition unit 76 in this embodiment, it is not necessary to select the measurement points M.

(Point Cloud Extraction)
FIG. 8 is a schematic diagram for explaining the extraction of the point cloud. The detection control unit 74 extracts the measurement points M to be superimposed by the point cloud superimposition unit 76 described later from the selected measurement points M, in other words, the measurement points M to be used to specify the position and orientation of the target P. That is, the detection control unit 74 selects the measurement points M included in the point cloud M0 acquired by the current detection by the sensor 26 based on the current incident angle θ, and then further selects the selected measurement points M to extract the measurement points M to be used to specify the position and orientation of the target P. The detection control unit 74 stores information on the extracted measurement points M in a buffer included in the storage unit 62. In this embodiment, the detection control unit 74 calculates a straight line L (position and orientation of the straight line L) corresponding to the front surface Pa of the target P based on the measurement points M selected based on the incident angle θ, and selects the measurement points M to be extracted based on the positional relationship between the straight line L and the measurement points M. Below, an example of the process of extracting the measurement points M will be specifically described.

(Getting line candidates)
As shown in FIG. 8, the detection control unit 74 sets the detection target region ROIa based on the position of the installation region AR0. The detection target region ROIa is a so-called ROI (Regions of Interest). The detection control unit 74 specifies a region that is estimated to include the front surface Pa of the target P based on the position of the installation region AR0, and sets the region as the detection target region ROIa. For example, the detection control unit 74 sets the detection target region ROIa having a width larger than the installation region AR0, and extracts the measurement points M in the detection target region ROIa from among the measurement points M included in the point cloud M0 selected based on the incident angle θ. The detection control unit 74 acquires one or more straight line candidates in order to detect the straight line L0 corresponding to the front surface Pa of the target P. The detection control unit 74 assigns a constant score to each of the one or more straight line candidates, which has a lower priority in selection than the other measurement points M, to the measurement points M that are closer to the front surface Pa than the straight line candidate by a specified distance or more. The detection control unit 74 selects a straight line L0 corresponding to the front surface Pa of the target P from among one or more straight line candidates, based on an integrated score value obtained by integrating the scores for the measurement points M in the detection target region ROIa.

Figure 9 is a schematic diagram showing a specific example of a line candidate. For example, as shown in Figure 9, the detection control unit 74 may further set a detection target area ROIb narrower than the detection target area ROIa, and acquire a line connecting two measurement points M selected from the point cloud M0 in the detection target area ROIb as a line candidate. Figure 9 shows an example in which the detection control unit 74 acquires four line candidates L1, L2, L3, and L4 based on the point cloud M0 in the detection target area ROIb. It is preferable that the detection target area ROIb is set to a width that does not allow adjacent luggage or the like to get in. The detection target area ROIb may be wider or narrower than the installation area AR0.

Here, a specific example of a method for acquiring line candidates will be described. The detection control unit 74 may be configured to acquire, as a line candidate, a line connecting two measurement points M selected from the point group M0 in the detection target area using the RANSAC algorithm. The detection target area in this case is the detection target area ROIb, but it may be the detection target area ROIa, or may be an area different from the detection target areas ROIa and ROIb. RANSAC (RANdom SAmple Consensus) is a method for randomly selecting two points and calculating an evaluation value. Note that the method for acquiring line candidates according to the present disclosure is not limited to the method using RANSAC, and other algorithms such as PROSAC (PROgressive SAmple Consensus) may be used.

The detection control unit 74 may be configured to acquire, as a line candidate, a straight line connecting two measurement points M selected from the point clouds M0 on the right and left sides of the center in the width direction of the detection target area. In this case, since the number of combinations is finite, all combinations of two points may be regarded as line candidates.

(Detection of line L0)
The detection control unit 74 calculates a score accumulation value for each of one or more line candidates, and detects a line L0 corresponding to the front surface Pa of the target P based on the score accumulation value. The detection control unit 74 selects (i.e. detects) a line candidate having a high priority indicated by the score accumulation value as the line L0, including a point group M0 outside the detection target region ROIb and within the detection target region ROIa, and may also subject such a point group M0 to subsequent processing. In the following, as an example, it is assumed that the detection control unit 74 selects the line candidate L1 shown in FIG. 9 as the line L0 based on the score accumulation value.

Here, we will explain the score accumulation value. For example, when a straight line candidate with a high score accumulation value is to be preferentially selected as the straight line to be used for line detection, the "constant score with low priority" means a score value that reduces the score accumulation value. On the other hand, when a straight line candidate with a low score accumulation value is to be preferentially selected, the "constant score with low priority" means a score value that increases the score accumulation value.

When calculating the score accumulation value, the detection control unit 74 uses the vertical distance between each measurement point M constituting the point cloud M0 and the straight line candidate. Specifically, the detection control unit 74 assigns a higher priority to measurement points M that are less than a specified distance from the straight line candidate than measurement points M that are more than a specified distance in front of the straight line candidate (closer to the position of the mobile body 10 at the time of detection), and the closer they are to the straight line candidate, the higher the priority. For example, the detection control unit 74 assigns a negative constant score to measurement points M that are more than a specified distance in front, and assigns a positive score to measurement points M that are less than the specified distance. The value of the positive score is a variable that depends on the distance from the straight line candidate.

Figure 10 is a graph showing an example of a score. The horizontal axis of this graph corresponds to the Y direction (depth) and indicates the distance between the measurement point M and the line candidate, with the further to the right along the horizontal axis indicating the deeper side (the side farther from the position of the moving body 10 at the time of detection). A position with a depth of zero is the position of the line candidate. The vertical axis indicates the value of the score to be assigned. In the example shown in Figure 10, a negative constant score is assigned to measurement points M that are a specified distance or more in front of the line candidate. In this way, a fixed penalty is imposed on measurement points M that are a specified distance or more in front of the line candidate, regardless of the distance between the line candidate and the measurement point M. A positive score that is larger is assigned to measurement points M that are less than the specified distance from the line candidate, the closer they are to the line candidate.

When calculating the score accumulation value, the detection control unit 74 excludes the measurement points M that are a specified distance or more behind the straight line candidate from the score accumulation value. "Excluding from the score accumulation value" means that the measurement points M are essentially excluded from the score accumulation value, and may be assigned a score of zero or may not be assigned a score to the measurement points M. In the example shown in FIG. 10, a score of zero is assigned to the measurement points M that are a specified distance or more behind the straight line candidate. When such a score accumulation value calculation method is adopted, in the example shown in FIG. 9, the straight line candidate L1 is selected as the straight line L0 line. The detection control unit 74 may detect multiple straight lines with high priorities indicated by the score accumulation value.

(Detection of line L)
The detection control unit 74 identifies the positions of both ends of the target P in the width direction, and detects the line segment that divides the straight line L0 at both ends as the straight line L. Specifically, the detection control unit 74 extracts one or more line segments from the straight line L0, and searches for one line segment or a combination of two or more line segments having both end lengths corresponding to the both end lengths of the target P determined from the design information from among the extracted one or more line segments. The "corresponding length" means a length within the range of the allowable error. The design information of the target P is information indicating the dimensions of the target P (e.g., the length of both ends, the position and interval of the opening PB, etc.), and is read out from, for example, the storage unit 62 of the control device 28. The detection control unit 74 regards the found line segment or combination of two or more line segments as the straight line L.

Figure 11 is a schematic diagram illustrating an example of line detection. As shown in Figure 11, for example, the detection control unit 74 extracts three line segments (measurement points M surrounded by circles) from the selected line L0, and searches for one line segment or a combination of two or more line segments having a double-end length WD that corresponds to the length of both ends of the target P. In the example shown in Figure 11, the double-end lengths of the combination of three line segments are detected as a result of the search, and it can be said that the line segment with double-end length WD along the line L0 is detected as the line L.

(Extraction of points based on straight line L)
The detection control unit 74 extracts the measurement points M to be superimposed by the point cloud superimposition unit 76 from among the measurement points M selected based on the incident angle θ based on the positional relationship between the straight line L and the measurement points M detected as described above. For example, the detection control unit 74 may extract the measurement points M within a predetermined distance range from the straight line L as the measurement points M to be superimposed by the point cloud superimposition unit 76. By extracting the measurement points M based on the positional relationship between the straight line L and the measurement points M in this way, the measurement points M that do not correspond to the front surface Pa of the target P can be removed, and the measurement points M that correspond to the front surface Pa of the target P can be appropriately extracted, thereby improving the detection accuracy of the position and orientation of the target P. In addition, the detection control unit 74 may extract the measurement points M estimated to be at the position of the pillar PA, excluding the measurement points M estimated to be at the position of the opening PB, based on the relative positions of the pillar PA and the opening PB on the front surface Pa of the target P acquired from the design information of the target P, etc.

In the above description, the detection control unit 74 detects the straight line L using the point cloud M0 acquired by one detection by the sensor 26, but this is not limited thereto, and the straight line L may be detected using the point cloud M0 acquired by multiple detections by the sensor 26. In other words, the detection control unit 74 may detect the straight line L based on the point cloud M0 acquired by at least one detection. Furthermore, the method of detecting the straight line L is not limited to the above method. Any method may be used.

The method of extracting the measurement points M is not limited to using the straight line L as described above, and the detection control unit 74 may extract the measurement points M by any method. Furthermore, the process of extracting the measurement points M itself is not essential, and all of the measurement points M selected based on the incident angle θ may be the measurement points M superimposed by the point cloud superimposition unit 76.

(Multiple detections)
The detection control unit 74 causes the sensor 26 to detect the front surface Pa of the target P multiple times while the moving body 10 is moving on the first path R1 (detection trajectory R1a). In other words, the detection control unit 74 causes the sensor 26 to scan the front surface Pa of the target P with the laser light LT multiple times while the moving body 10 is moving on the first path R1 (detection trajectory R1a). The detection control unit 74 acquires the multiple detection results by the sensor 26 as a point cloud M0. For each detection result of the sensor 26 (each time the sensor 26 detects), the detection control unit 74 executes the above-described process of acquiring, selecting, and extracting the measurement point M.

In this manner, in this embodiment, the sensor 26 performs detection multiple times while moving on the first path R1. That is, the detection control unit 74 causes the sensor 26 to perform detection for each position of the moving body 10 on the first path R1, so the positions of the moving body 10 when each detection result is obtained are different from each other. However, the timing at which the sensor 26 is caused to perform detection is not limited to when the moving body 10 is moving on the first path R1. For example, the sensor 26 may 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 detection multiple times, and may cause multiple sensors 26 to detect the same target P, thereby obtaining multiple detection results of the sensor 26 and forming the point cloud M0.

(Point cloud superposition)
FIG. 12 is a schematic diagram for explaining a specific example of superimposition of point clouds. As shown in FIG. 12, the point cloud superimposition unit 76 superimposes the measurement points M (point cloud M0) extracted by the detection control unit 74 in the same coordinate system. In other words, the point cloud superimposition unit 76 acquires information on the positions (coordinates) in the same coordinate system of the measurement points M extracted by the detection control unit 74 among the measurement points M corresponding to the multiple detection results by the sensor 26. The same coordinate system in this embodiment is a two-dimensional coordinate system CO in the direction X and the direction Y, and can also be said to be a coordinate system in the equipment W. That is, the point cloud superimposition unit 76 superimposes each measurement point M extracted by the detection control unit 74 in the two-dimensional coordinate system CO (plots on the two-dimensional coordinate system CO). Note that when the measurement points M are calculated as positions in the two-dimensional coordinate system CO, the coordinate systems of the respective measurement points M are common, so that the point cloud superimposition unit 76 can acquire information on the positions of the respective measurement points M in the two-dimensional coordinate system CO without performing a coordinate system conversion process for each measurement point M. However, for example, when each measurement point M is not calculated as a position in the same coordinate system, such as a coordinate system based on the moving body 10, the point cloud superimposition unit 76 performs coordinate conversion of each measurement point M based on the position in the two-dimensional coordinate system CO of the moving body 10 when the measurement point M is detected, and calculates the position of the measurement point M in the two-dimensional coordinate system CO. Note that the coordinate system on which each measurement point M is superimposed is not limited to the two-dimensional coordinate system CO, and may be, for example, a three-dimensional coordinate system.

(Determining the target position and attitude)
The target information acquisition unit 78 specifies the position and orientation of the target P based on the positions of the measurement points M (point cloud M0) superimposed on the two-dimensional coordinate system CO by the point cloud superimposition unit 76. A specific example of a method for specifying the position and orientation of the target P based on the superimposed measurement points M will be described below.

(Calculation of the approximation line)
FIG. 13 is a schematic diagram showing an example of an approximation line. In this embodiment, the target information acquisition unit 78 calculates an approximation line N (the position and orientation of the approximation line N in the two-dimensional coordinate system CO) of each superimposed measurement point M, and specifies the position and orientation of the target P based on the approximation line N. As shown in FIG. 13, the approximation line N is an approximation line of each superimposed measurement point M, and can be said to be an approximation line of each measurement point M corresponding to a plurality of detection results by the sensor 26. In this embodiment, the target information acquisition unit 78 calculates the approximation line N by performing straight line fitting to each superimposed measurement point M by the least squares method. However, the calculation method of the approximation line N is not limited to the least squares method, and any method may be used. For example, the straight line L calculated based on the point group M0 acquired by the first detection by the sensor 26 may be used as the approximation line N, or multiple straight lines may be generated using RANSAC using each superimposed measurement point M, and a straight line close to the front surface Pa may be used as the approximation line N from the gradient distribution of each straight line. The method of generating a straight line using RANSAC may be the same as the method of generating a straight line candidate described above. In this embodiment, the front surface Pa of the target P is flat, so the approximation line N is calculated as a straight line, but the approximation line is not limited to being a straight line. For example, when it is known that the front surface Pa is a curved surface, the approximation line N may be calculated as a curved line accordingly.

(Projection of measurement points)
14 is a schematic diagram showing an example of measurement points projected onto an approximation line. The target information acquisition unit 78 converts the measurement point M in the two-dimensional coordinate system CO into a measurement point MA in the coordinate system of the approximation line N as shown in the example of FIG. 14 by projecting each of the superimposed measurement points M (point group M0) onto the approximation line N. In other words, the measurement point MA can be said to be the measurement point M in the two-dimensional coordinate system CO that has been transformed into the coordinate system of the approximation line N by projective transformation. Note that the coordinate system of the approximation line N is a one-dimensional coordinate, and the measurement point MA in the coordinate system of the approximation line N indicates the position of the measurement point in the direction along the approximation line N. For example, the target information acquisition unit 78 may draw a perpendicular line from the measurement point M to the approximation line N, and the intersection of the perpendicular line and the approximation line N may be the measurement point MA.

In this embodiment, the target information acquisition unit 78 identifies the orientation of the target P based on the approximation line N, and identifies the position of the target P based on the positions of the measurement points MA (point cloud) in the coordinate system of the approximation line N. More specifically, the target information acquisition unit 78 calculates the number of measurement points MA for each position based on the position of each measurement point MA, and clusters the measurement points MA based on the number of measurement points MA for each position to identify the position of the target P. A specific example of this process is described below.

(Calculation of the number of measurement points per position)
FIG. 15 is a schematic diagram showing an example of a histogram of measurement points. Based on the measurement points MA in the coordinate system of the approximation line N, the target information acquisition unit 78 calculates the number of measurement points MA for each position in the coordinate system of the approximation line N. For example, the target information acquisition unit 78 divides the coordinate system of the approximation line N into a plurality of unit ranges and assigns each measurement point MA to one of the unit ranges. That is, the target information acquisition unit 78 assigns the measurement point MA to a unit range that includes the position of the measurement point MA in the coordinate system of the approximation line N. The target information acquisition unit 78 executes a process of assigning the measurement point MA to a unit range for each measurement point MA, and calculates the number of measurement points MA for each unit range. FIG. 14 is a histogram showing an example of the number of measurement points MA for each unit range. The target information acquisition unit 78 may generate a histogram showing the number of measurement points MA for each unit range in the coordinate system of the approximation line N, as shown in FIG. 15.

(Clustering of measurement points)
Based on the number of measurement points MA for each position in the coordinate system of the approximation line N, the target information acquisition unit 78 clusters (classifies) the measurement points MA in the coordinate system of the approximation line N into the number of pillars PA of the target P, thereby generating clusters (a set of measurement points MA) for the number of pillars PA. That is, based on the number of measurement points MA for each position in the coordinate system of the approximation line N, the target information acquisition unit 78 specifies which measurement point MA indicates the position of which pillar PA of the target P. Specifically, the target information acquisition unit 78 extracts ranges RA including unit ranges in which the number of measurement points MA is equal to or greater than a predetermined number, the number of ranges being equal to the number of pillars PA on the front surface Pa. Then, the target information acquisition unit 78 classifies the measurement points MA included in the same range RA into the same cluster. Furthermore, the range RA here preferably refers to a range (section) in the coordinate system of the approximation line N in which the number of measurement points MA is equal to or greater than a predetermined number in all unit ranges included in the range RA. In this case, the ranges RA do not overlap with each other. The predetermined number here may be set arbitrarily, but may be determined based on the maximum number of measurement points MA per unit range, for example, and may be set to, for example, 1/3 of the maximum value as the predetermined number.

In the example of this embodiment, as shown in FIG. 12, the target P has three pillars PA on the front surface Pa: a central pillar PA1 in the width direction (Y direction in the example of FIG. 12), a pillar PA2 on one side of the pillar PA1 in the width direction, and a pillar PA3 on the other side of the pillar PA1. Therefore, in the example of this embodiment, the target information acquisition unit 78 extracts three ranges RA1, RA2, and RA3 as ranges RA in which the number of measurement points MA is a predetermined number or more, as shown in FIG. 15. Then, the target information acquisition unit 78 clusters the measurement points MA included in the range RA1 located in the center in the coordinate system of the approximation line N as measurement points corresponding to the pillar PA1, clusters the measurement points MA included in the range RA2 on one side of the range RA1 as measurement points corresponding to the pillar PA2, and clusters the measurement points MA included in the range RA3 on the other side of the range RA1 as measurement points corresponding to the pillar PA3. However, the number of pillars PA is not limited to three and is arbitrary, and the number of clusters is determined according to the number of pillars PA. Furthermore, the method for clustering the measurement points MA is not limited to the above, and any method may be used.

(Determining the target position and attitude)
FIG. 16 is a schematic diagram for explaining an example of a method for identifying the position of a target object. As described above, the target information acquisition unit 78 identifies the orientation of the target object P in the two-dimensional coordinate system CO based on the approximation line N. For example, the target information acquisition unit 78 determines the inclination of the approximation line N in the two-dimensional coordinate system CO as the orientation of the target object P. The target information acquisition unit 78 also identifies the position of the target object P in the two-dimensional coordinate system CO based on the positions of the clustered measurement points MA. Specifically, the target information acquisition unit 78 calculates the positions of both end points of the front surface Pa of the target object P as the position of the target object P based on the positions of the clustered measurement points MA. Then, the target information acquisition unit 78 identifies the line segment dividing the approximation line N at the calculated both end points as the position and orientation of the front surface Pa of the target object P. Note that both end points of the front surface Pa refer to the end points on one side and the other side in the width direction of the front surface Pa.

In this embodiment, the target information acquisition unit 78 calculates a reference position C for each cluster. Then, the target information acquisition unit 78 determines whether the calculated reference position C can be used to identify the position of the target P based on the distance between the reference positions C for each cluster. Since the clusters are classified by pillar PA, the reference position C can be said to be a position corresponding to the position of each pillar PA. The target information acquisition unit 78 determines whether the distance between the reference positions C is within a predetermined distance range, and if it is within the predetermined distance range, determines that the reference position C is used to identify the position of the target P. In this case, the target information acquisition unit 78 identifies the position of the target P based on the calculated reference position C. For example, the target information acquisition unit 78 may use at least one of the calculated reference positions C to calculate the center position in the width direction of the front surface Pa of the target P, and calculate the positions of both end points of the front surface Pa of the target P based on the center position. On the other hand, if the distance between the reference positions C is outside the predetermined distance range, that is, if the distance is shorter than the predetermined distance range, or if the distance is longer than the predetermined distance range, it is determined that the reference position C is not used to identify the position of the target P. In this case, the calculated reference position C is not used to identify the position of the target P, and, for example, the detection process by the sensor 26 is performed again to redo the process of identifying the position and orientation of the target P. The predetermined distance range may be set arbitrarily, but may be, for example, a numerical range that includes a predetermined margin on the distance between the center positions of the pillars PA in the design value.

In the example of this embodiment, the target information acquisition unit 78 calculates the position of the center of gravity of the measurement points MA of each cluster as the reference position C. The target information acquisition unit 78 judges whether the distance between the centers of gravity is within a predetermined distance range, and if it is within the predetermined distance range, judges that the position of the center of gravity is used to identify the reference position C of the target P. Then, the target information acquisition unit 78 identifies the position of the target P based on the calculated position of the center of gravity. In the example of FIG. 16, the center of gravity of the measurement points MA included in the cluster M0A1 corresponding to the pillar PA1 is the reference position C1, the center of gravity of the measurement points MA included in the cluster M0A2 corresponding to the pillar PA2 is the reference position C2, and the center of gravity of the measurement points MA included in the cluster M0A3 corresponding to the pillar PA3 is the reference position C3. In this case, the target information acquisition unit 78 judges whether the distance between the reference position C1 and the reference position C2 and the distance between the reference position C1 and the reference position C3 are within a predetermined distance range. If the distance between the reference position C1 and the reference position C2 and the distance between the reference position C1 and the reference position C3 are within a predetermined distance range, the target information acquisition unit 78 calculates the position of the target P by setting the reference position C1 as the center position of the front surface Pa of the target P. On the other hand, if at least one of the distance between the reference position C1 and the reference position C2 and the distance between the reference position C1 and the reference position C3 is outside the predetermined distance range, the calculated reference position C1 is not used to identify the position of the target P, and the target information acquisition unit 78 redoes the process of identifying the position and orientation of the target P, for example, by causing the sensor 26 to perform the detection process again.

Note that since the measurement point MA is a position in the coordinate system of the approximation line N, the reference position C is also calculated as a position in the coordinate system of the approximation line N. Therefore, the target information acquisition unit 78 may convert the reference position C in the coordinate system of the approximation line N into a two-dimensional coordinate system CO in the X and Y directions to determine the central position of the target P. However, the target information acquisition unit 78 may also extract measurement points M in the two-dimensional coordinate system CO corresponding to the measurement points MA of each cluster, and calculate the reference position C of the target P in the two-dimensional coordinate system CO based on the extracted measurement points M for each cluster. In this case, since the reference position C of the target P is calculated as a coordinate on the two-dimensional coordinate system CO, subsequent coordinate conversion is not necessary.

The target information acquisition unit 78 identifies the position and orientation of the target P in the manner described above. However, the method by which the target information acquisition unit 78 identifies the position and orientation of the target P is not limited to the above description, and the target information acquisition unit 78 may identify the position and orientation of the target P in any manner based on the positions of the measurement points M (point cloud M0) superimposed on the two-dimensional coordinate system CO.

(Second pass setting)
FIG. 17 is a schematic diagram showing an example of movement along the second path. The second path information acquisition unit 80 sets the target position A3 from the position and posture of the target P identified by the target information acquisition unit 78. For example, the second path information acquisition unit 80 calculates the position and posture at which the target P can be picked up (the fork 24 can be inserted into the opening PB of the target P by moving straight) from the position and posture of the target P, and sets it as the target position A3. As an example, a position moved 1000 mm in parallel from the entrance of the opening PB in the axial direction of the opening PB of the target P may be set as the target position A3. Then, as shown in FIG. 17, the second path information acquisition unit 80 sets the trajectory from the second position A2, which is the start position, to the set target position A3 as the second path R2. The second path information acquisition unit 80 may set the second path R2 by any method based on the information of the target position A3, but may also calculate the second path R2 by model predictive control (MPC), for example.

(Movement along the second path)
The movement control unit 72 moves the moving body 10 from the second position A2 to the target position A3, passing through the second path R2. When the moving body 10 arrives at the target position A3, the movement control unit 72 moves the moving body 10 straight from the target position A3 and inserts the fork 24 into the opening PB of the target object P to pick up the target object P. The movement control unit 72 transports the moving body 10, which has picked up the target object P, to a set destination.

In this way, the movement control unit 72 moves the moving body 10 along the second path R2 from the second position A2 to the target position A3, but is not limited thereto. For example, the moving body 10 may be moved to the target position A3 by switching between movement along the second path R2 and movement by direct feedback control. An example of control by direct feedback is control by visual servoing, as described in "Ozato Jun, Maru Noriaki, "Position and Posture Control of Omnidirectional Mobile Robot by Linear Visual Servoing," Transactions of the Japan Society of Mechanical Engineers (C), Vol. 77, No. 774, pp. 215-224, February 25, 2011." Also, for example, when the moving body 10 arrives at the target position A3 along the second path R2, the movement control unit 72 may switch to control by direct feedback and move the moving body 10 to pick up the target object P.

(Processing flow for detecting the position and attitude of a target object)
Next, a process flow for detecting the position and attitude of the target P described above will be described. FIG. 18 is a flowchart for explaining a process flow for detecting the position and attitude of the target. The detection control unit 74 of the control device 28 causes the sensor 26 to detect the target P multiple times while the moving body 10 moves along the first path R1 from the first position A1 to the second position A2. That is, in this embodiment, the sensor 26 detects the target P while moving along the first path R1. As shown in FIG. 18, the detection control unit 74 acquires a point cloud M0 (measurement points M) from the detection result by the sensor 26 (step S10). Then, the detection control unit 74 determines whether there are any measurement points M that have been acquired by detection other than the current detection, that is, whether there are any measurement points M superimposed by the point cloud superimposition unit 76 in this embodiment (step S12). If there are superimposed measurement points M (step S12; Yes), the detection control unit 74 calculates an approximation line K of the superimposed measurement points M (step S14), calculates the incidence angle θ for each measurement point M (step S16), and selects a measurement point M from the measurement points M obtained in this detection based on the incidence angle θ (step S18).

Then, the detection control unit 74 calculates a straight line L corresponding to the front surface Pa of the target P based on the selected point group M0 (step S20). In this embodiment, the detection control unit 74 detects straight line candidates from the measurement points M and calculates the straight line L0 based on the score accumulation value. Then, the detection control unit 74 identifies the positions of both end points in the width direction of the target P and detects the line segment dividing the straight line L0 at both end points as the straight line L. If both end points cannot be identified, the process may proceed to step S18 described later without extracting and superimposing the measurement points M. After detecting the straight line L, the detection control unit 74 extracts some of the measurement points M from the acquired measurement points M based on the straight line L (step S22). In this embodiment, the detection control unit 74 extracts the measurement points M based on the positional relationship between the measurement points M and the straight line L. If there are no superimposed measurement points M (step S12; No), i.e., if there are no measurement points M obtained by detection other than the current detection, the process proceeds to step S20 without selecting a measurement point M, and the measurement points M obtained in the current detection are used to calculate a straight line L and extract the measurement points M.

The point cloud superimposition unit 76 of the control device 28 superimposes the measurement points M extracted by the detection control unit 74 in the same coordinate system (here, the two-dimensional coordinate system CO) (step S24). In step S24, if the movement distance of the moving body 10 from the previous detection by the sensor 26 to the current detection is equal to or greater than a predetermined distance, the measurement points M are superimposed, and if the movement distance is less than the predetermined distance, the process may proceed to step S26 described below without superimposing the measurement points M. The movement distance may be obtained by any method, but may be obtained, for example, by calculating the movement distance from the previous detection by the sensor 26 to the current detection from the position of the moving body 10 when the sensor 26 performed the previous detection and the position of the moving body 10 when the sensor 26 performed the current detection.

The point cloud superimposition unit 76 determines whether the number of measurement points M superimposed in the same coordinate system is sufficient (step S26), and if it is not determined that the number is sufficient (step S26; No), that is, if it is determined that the number of measurement points M is not sufficient, the process returns to step S10, and the point cloud M0 is acquired by another detection result (detection result by another scan) by the sensor 26, thereby repeating the acquisition of the point cloud M0. By repeating the detection of the sensor 26 in this way until the number of measurement points M becomes sufficient, the point cloud superimposition unit 76 can superimpose a sufficient number of measurement points M corresponding to the multiple detection results of the sensor 26 in the same coordinate system. Note that the determination of whether the number of measurement points M is sufficient may be performed by any method. For example, the point cloud superimposition unit 76 may use as a determination criterion whether the number of measurement points M is equal to or greater than a threshold. In this case, for example, if the number of superimposed measurement points M is equal to or greater than a threshold, the point cloud superimposition unit 76 determines that the number of measurement points M is sufficient, and if the number is less than the threshold, determines that the number of measurement points M is not sufficient. In this case, the threshold value may be set to any number, but is preferably set to a number greater than the number of typical measurement points M extracted in one detection (scan). Also, for example, the point cloud superimposition unit 76 may use the number of detections (number of scans) by the sensor 26 as a judgment criterion. In this case, for example, if the number of detections by the sensor 26 is equal to or greater than a predetermined number, the point cloud superimposition unit 76 judges that the number of measurement points M is sufficient, and if the number is less than the predetermined number, it judges that the number of measurement points M is insufficient. The predetermined number here may be set to any number greater than or equal to two, for example.

In this embodiment, each time the sensor 26 detects once, the process of acquiring, selecting, extracting, and superimposing the measurement points M is performed, and in step S26, it is determined whether a sufficient number of measurement points M have been superimposed, but this is not limited to the above. For example, the process of acquiring, selecting, extracting, and superimposing the measurement points M may be performed after the sensor 26 detects any number of times, and in step S26, it may be determined whether a sufficient number of measurement points M have been superimposed.

If it is determined that the number of measurement points M is sufficient (step S26; Yes), the target information acquisition unit 78 of the control device 28 calculates an approximation line N of the superimposed measurement points M based on the positions of each measurement point M (point cloud M0) superimposed by the point cloud superimposition unit 76 (step S28). The target information acquisition unit 78 converts each measurement point M superimposed by the point cloud superimposition unit 76 into the coordinate system of the approximation line N (step S30) and acquires the measurement points MA in the coordinate system of the approximation line N. Then, the target information acquisition unit 78 clusters the measurement points MA and calculates the reference position C (step S32). In this embodiment, the target information acquisition unit 78 classifies the measurement points MA by cluster and calculates the reference position C for each cluster. Then, the target information acquisition unit 78 determines whether the distance between the reference positions C is within a predetermined distance range (step S34), and if it is not within the predetermined distance range (step S34; No), returns to step S10. Also, if clustering cannot be performed (for example, if classification into clusters equal to the number of pillars PA is not possible), you may return to step S10.

If the distance between the reference positions C is within a predetermined distance range (step S34; Yes), the target information acquisition unit 78 identifies the position and orientation of the target P based on the approximation line N and the reference position C (step S36). In this embodiment, the target information acquisition unit 78 identifies the orientation of the target P based on the approximation line N, and identifies the position of the target P based on the reference position C. Specifically, in this embodiment, the target information acquisition unit 78 identifies the center of gravity of each cluster of measurement points MA as the reference position C and calculates the distance between the centers of gravity. Then, if the distance between the centers of gravity is within a predetermined distance range, the target information acquisition unit 78 calculates the positions of both end points of the front surface Pa of the target P from the reference position C of the target P and the approximation line N, and identifies the line segment dividing the approximation line N by the calculated end points as the front surface Pa of the target P.

Once the position and orientation of the target P have been identified in step S36, the control device 28 generates a second path R2 based on the position and orientation of the target P, and moves the mobile body 10 along the second path R2 to approach the front surface Pa of the target P.

(effect)
As described above, in the moving body 10 according to this embodiment, the detection control unit 74 calculates the approximation line K based on the measurement points M acquired in detections other than the current detection, and calculates the incidence angle θ of the laser light LT in the current detection based on the approximation line K, the position AR1 of the moving body 10, and the position of the measurement points M. Then, the detection control unit 74 selects the measurement points M used to identify the position and orientation of the target P based on the incidence angle θ. Therefore, according to this embodiment, it is possible to exclude the measurement points M with low detection accuracy and use the measurement points M with high detection accuracy to identify the position and orientation of the target P. Therefore, according to this embodiment, the decrease in the detection accuracy of the target P can be suitably suppressed. In particular, it is remarkable that the detection accuracy of the sensor 26 that detects the target P by irradiating light changes depending on the incidence angle of the light to the target P. On the other hand, as in this embodiment, the incidence angle θ is estimated and the measurement points M to be used are selected based on the estimated incidence angle, thereby suitably suppressing the decrease in the detection accuracy.

In this embodiment, the control device 28 of the moving body 10 calculates the approximation line K, calculates the incident angle θ, selects the measurement point M based on the incident angle θ, and identifies the position and attitude of the target P based on the selected measurement point M. However, the processing performed by the control device 28 described above is not limited to being performed by the moving body 10, and other devices such as the information processing device 14 may execute at least a part of these processes, and the moving body 10 may acquire the results. That is, for example, the detection control unit 74 of the moving body 10 may acquire information on the approximation line K, may calculate the approximation line K by itself, or may acquire information on the calculated approximation line K from an external device. Also, for example, the detection control unit 74 of the moving body 10 may acquire information on the incident angle θ, may calculate the incident angle θ by itself, or may acquire the calculation result of the incident angle θ from an external device. When the detection control unit 74 acquires the calculation result of the incident angle θ from an external device, the moving body 10 does not need to perform calculations using the approximation line K, so it can be said that it is not necessary to acquire information on the approximation line K. Also, for example, the detection control unit 74 of the moving body 10 may acquire information on the measurement points M selected based on the incident angle θ, may select the measurement points M based on the incident angle θ by itself, or may acquire information on the selected measurement points M from an external device. When the detection control unit 74 acquires information on the selected measurement points M from an external device, the moving body 10 does not need to select the measurement points M based on the incident angle θ, so it can be said that it is not necessary to acquire information on the incident angle θ. Also, for example, the target information acquisition unit 78 of the moving body 10 may acquire information on the position and attitude of the target P, may specify the position and attitude of the target P by itself, or may acquire information on the position and attitude of the target P from an external device. When the target information acquisition unit 78 acquires information on the position and attitude of the target P from an external device, the moving body 10 does not need to perform calculations using information on the selected measurement points M, so it can be said that it is not necessary to acquire information on the selected measurement points M. Also, for example, the second path information acquisition unit 80 of the moving body 10 may acquire information on the second path R2, may set the second path R2 on its own based on the position and orientation of the target P, or may acquire information on the second path R2 from an external device. When the second path information acquisition unit 80 acquires information on the second path R2 from an external device, the moving body 10 does not need to perform calculations using the position and orientation of the target P, so it can be said that there is no need to acquire information on the superimposed point group M0 or information on the position and orientation of the target P.

(Modification)
In the first embodiment, the center of gravity of the measurement points MA for each cluster is used as the reference position C used to identify the position of the target P, but the present invention is not limited to using the center of gravity as the reference position C. Modifications of the first embodiment will be described below. In the following modifications, descriptions of points common to the first embodiment will be omitted.

(First Modification)
FIG. 19 is a schematic diagram for explaining an example of a method for identifying the position of a target in the first modified example. In the first modified example, the target information acquisition unit 78 sets a predetermined section SE and a detection section RA. The predetermined section SE is a section that is a predetermined range set in advance in the coordinate system of the approximation line, in other words, a section that is a predetermined length along the approximation line. The length of the predetermined section SE may be set arbitrarily. The detection section RA is a section that is a predetermined range set in advance in the coordinate system of the approximation line and is a predetermined position (coordinate) set in advance. The detection section RA is a position where the pillar PA is assumed to exist, and the position of the detection section RA may be set in an arbitrary manner. In addition, the range (length) of the detection section RA may also be set arbitrarily, but is set, for example, longer than the range (length) of the predetermined section SE. The detection section RA is set to the number of pillars PA of the target P, that is, the number of clusters of the measurement points MA. In the example of Fig. 19, a detection section RA1 corresponding to the central pillar PA1, a detection section RA2 corresponding to the pillar PA2 on one side, and a detection section RA3 corresponding to the pillar PA3 on the other side are set. The position and length of the detection section RA may be set based on the position of the cluster of the measurement point MA so that the detection section RA includes the cluster. In this case, for example, the detection section RA1 may be set to include the cluster M0A1 corresponding to the central pillar PA1, the detection section RA2 may be set to include the cluster M0A2 corresponding to the pillar PA2 on one side, and the detection section RA3 may be set to include the cluster M0A1 corresponding to the pillar PA3 on the other side.

As shown in the example of FIG. 19, the target information acquisition unit 78 of the first modified example moves the position of the specified section SE within the range of the detection section RA, and calculates the number of cluster measurement points, which is the number of measurement points MA included in the range of the specified section SE, each time the position of the specified section SE is moved. That is, the target information acquisition unit 78 calculates the number of cluster measurement points (the number of measurement points MA included in the range of the specified section SE) for each position of the specified section SE. For example, the target information acquisition unit 78 moves the position of the specified section SE by the length of a unit range (the range in which the number of measurement points MA is counted) and calculates the number of cluster measurement points in each specified section SE whose position has shifted by the length of the unit range. However, the distance by which the position of the specified section SE is moved is not limited to the length of the unit range, and it can be said that the target information acquisition unit 78 moves the position of the specified section SE by the specified length and calculates the number of cluster measurement points in each specified section SE whose position has shifted by the specified length.

The target information acquisition unit 78 of the first modified example specifies the reference position C based on the number of cluster measurement points. For example, the target information acquisition unit 78 specifies the reference position C based on the position of the specified section SE where the number of cluster measurement points is maximum among the specified sections SE for each position within the range of the detection section RA. In this case, for example, the target information acquisition unit 78 may set the reference position C to the position (e.g., the center position) of the specified section SE where the number of cluster measurement points is maximum. The target information acquisition unit 78 specifies the reference position C for each detection section RA, that is, for each cluster, in a similar manner. Therefore, in the example of FIG. 19, the center position of the specified section SE where the number of cluster measurement points is maximum in the detection section RA1 is set to the reference position C1, the center position of the specified section SE where the number of cluster measurement points is maximum in the detection section RA2 is set to the reference position C2, and the center position of the specified section SE where the number of cluster measurement points is maximum in the detection section RA3 is set to the reference position C3.

Once the reference position C is identified, the position of the target P is identified in the first modified example in a manner similar to that of the first embodiment, so further explanation is omitted.

As in the first modified example, by calculating the number of cluster measurement points for each position in the specified section SE and setting a reference position for each cluster based on the number of cluster measurement points, the position of the target P can be calculated with high accuracy.

(Second Modification)
FIG. 20 is a schematic diagram for explaining an example of a method for identifying the position of a target in the second modified example. In the second modified example, the target information acquisition unit 78 sets a predetermined section SE and a detection section RA in the coordinate system of the approximation line. The second modified example differs from the first embodiment in that the predetermined section SE is set for each pillar PA, that is, for each cluster. In the example of FIG. 20, the target information acquisition unit 78 sets a predetermined section SE1 (first section), a predetermined section SE2 (second section), and a predetermined section SE3 (third section). The predetermined sections SE1, SE2, and SE3 are sections that are predetermined ranges set in advance in the coordinate system of the approximation line, in other words, sections that have a predetermined length along the approximation line. The length of the predetermined section SE may be set arbitrarily. However, as described later, when calculating the cluster measurement points, the positions of the predetermined section SE1, the predetermined section SE2, and the predetermined section SE3 are moved while the distance (i.e., the relative positions) between them is fixed. That is, for example, if the distance between the predetermined section SE1 and the predetermined section SE2 is DIa, and the distance between the predetermined section SE1 and the predetermined section SE3 is DIb, even if the predetermined sections SE1, SE2, and SE3 are moved to calculate the cluster measurement points, the distance between the predetermined section SE1 and the predetermined section SE2 is fixed as the distance DIa, and the distance between the predetermined section SE1 and the predetermined section SE3 is fixed as the distance DIb. The detection section RA is a section that is a predetermined position and a predetermined range that is set in advance in the coordinate system of the approximation line. In the second modified example, the detection section RA is not limited to being set to the number of pillars PA of the target P, and it is sufficient that one detection section is set.

20, the target information acquisition unit 78 of the second modified example fixes the distance (relative position) between the predetermined sections SE1, SE2, and SE3, and calculates the number of cluster measurement points for the predetermined sections SE1, SE2, and SE3 each time the position of the predetermined sections SE1, SE2, and SE3 is moved while moving the positions in the coordinate system of the approximation line N of the predetermined sections SE1, SE2, and SE3 within the range of the detection section RA. That is, the target information acquisition unit 78 calculates the number of cluster measurement points for each position of the predetermined sections SE1, SE2, and SE3. For example, the target information acquisition unit 78 moves the positions of the predetermined sections SE1, SE2, and SE3 by the length of the unit range to which the number of measurement points MA is assigned, and calculates the number of cluster measurement points in each of the predetermined sections SE1, SE2, and SE3 whose positions are shifted by the length of the unit range. However, the distance by which the positions of the specified sections SE1, SE2, and SE3 are moved is not limited to a unit range, and the target information acquisition unit 78 can be said to move the positions of the specified sections SE1, SE2, and SE3 by a specified length and calculate the cluster measurement points in each of the specified sections SE1, SE2, and SE3 whose positions are shifted by the specified length.

In the second modified example, the target information acquisition unit 78 identifies the reference position C based on the cluster measurement points. For example, the target information acquisition unit 78 identifies the reference positions C1, C2, and C3 based on the positions of the predetermined sections SE1, SE2, and SE3 where the total value of the cluster measurement points in the predetermined sections SE1, SE2, and SE3 is maximum among the predetermined sections SE1, SE2, and SE3 for each position within the range of the detection section RA. For example, the target information acquisition unit 78 calculates the total value of the cluster measurement points in the predetermined sections SE1, SE2, and SE3 for each position of the predetermined sections SE1, SE2, and SE3, and identifies the reference positions (for example, central positions) of the predetermined sections SE1, SE2, and SE3 when the total value of the cluster measurement points is maximum as the reference positions C1, C2, and C3. In the example of FIG. 20, the target information acquisition unit 78 sets the center positions of the predetermined sections SE1, SE2, and SE3 where the total value of the cluster measurement points is the maximum as the reference positions C1, C2, and C3, respectively.

Once the reference position C is identified, the position of the target P is identified in the second modified example in a manner similar to that of the first embodiment, so further explanation is omitted.

As in the second modified example, by setting a reference position based on the number of cluster measurement points for each position in the specified section SE, the position of the target P can be calculated with high accuracy. Furthermore, in the second modified example, the number of cluster measurement points is calculated while fixing the relative positions of the specified sections SE1, SE2, and SE3, so the process of clustering the measurement points MA for each pillar PA (the process of dividing the measurement points MA into clusters) is not necessary.

(Other examples)
In this embodiment, the moving body 10 detects the same target P every time it moves, but the method of acquiring the multiple detection results is not limited to this. For example, the detection control unit 74 may cause the multiple sensors 26 of the moving body 10 to detect the same target P and acquire the measurement results of each sensor 26 as measurement points M (point cloud M0) acquired by multiple detections. The measurement points M acquired by the detection of each sensor 26 are selected and extracted in the same manner as described in the first embodiment, and are superimposed on the same coordinate system (here, the two-dimensional coordinate system CO) by the point cloud superimposition unit 76.

Therefore, in this case, the measurement points M used to calculate the approximation line K (measurement points M obtained by a detection different from the current detection) also include measurement points M obtained by detection of a different sensor 26. That is, in this example, the detection control unit 74 calculates the approximation line K using measurement points M obtained by detection of a sensor 26 different from the sensor 26 used in the current detection, calculates the incidence angle θ in the current detection based on the approximation line K, and selects the measurement points M.

In this way, by performing detection using multiple sensors 26 and calculating the approximation line K using the detection results of the other sensors 26, it is possible to perform calculation processing quickly. In addition, since the multiple sensors 26 are located at different positions, the incidence angle θ at the same position on the target P is different for each sensor 26. Therefore, it is possible to select the measurement points M on the entire front surface Pa of the target P without the positions of the measurement points M being excluded from selection because the incidence angle θ falls outside the predetermined range being concentrated on the front surface Pa of the target P. Therefore, even in this case, the decrease in detection accuracy can be more appropriately suppressed.

(Effects of the present disclosure)
As described above, the control method for the moving body 10 in the present disclosure is a control method for the moving body 10 that moves automatically, and includes the steps of: detecting the target P multiple times by irradiating the sensor 26 provided on the moving body 10 with light, and acquiring the results of the multiple detections by the sensor 26 as a point cloud M0; detecting an approximation line K of the point cloud M0 based on the first point cloud, which is the point cloud M0 corresponding to the first detection of the sensor 26; calculating an incidence angle θ of light from the sensor 26 to the target P in the second detection based on the approximation line K, the second point cloud, which is the point cloud M0 corresponding to the second detection of the sensor 26, and the position AR1 of the moving body 10 when the second detection is performed; selecting a point cloud M0 for specifying the position and orientation of the target P from the second point cloud based on the incidence angle θ; and specifying the position and orientation of the target P based on the point cloud M0 selected from the second point cloud. According to this control method, it is possible to specify the position and orientation of the target P by excluding measurement points M with low detection accuracy and using measurement points M with high detection accuracy. Therefore, according to this control method, the decrease in the detection accuracy of the target P can be effectively suppressed.

The control method disclosed herein further includes the steps of setting an approach path (second path R2) to a target position A3 that is at a predetermined position and orientation relative to the target P based on the position and orientation of the target P, and moving the mobile body 10 along the second path R2. The control method disclosed herein sets the second path R2 based on a point cloud M0 acquired from multiple detection results by the sensor 26, so that the second path R2 can be generated with high accuracy, making it possible to appropriately approach the target P.

The control method disclosed herein further includes a step of acquiring information on a wide area path (first path R1) that crosses the installation area AR0 in which the target P is installed, and a step of moving the moving body 10 along the first path R1. In the step of acquiring the point cloud M0, the sensor 26 is caused to detect the target P multiple times while the moving body 10 is moving along the first path R1. According to this control method, by causing the sensor 26 to detect the target P multiple times while moving across the target P, the target P can be appropriately detected, and a decrease in the detection accuracy of the target P can be further preferably suppressed.

In the step of calculating the incidence angle θ, the incidence angle θ is calculated for each measurement point M included in the second point cloud, and in the step of selecting the point cloud M0, measurement points M included in the second point cloud whose incidence angle θ is within a predetermined range are selected. The type of sensor 26 that detects the target P by irradiating light has a noticeable change in detection accuracy depending on the incidence angle of the light on the target P, and there is an angle band where the detection accuracy is low, for example, when the incidence angle is small. In contrast, by selecting and using measurement points M whose incidence angle θ is within a predetermined range as in the present embodiment, it is possible to exclude measurement points M with low detection accuracy and further preferably suppress the decrease in the detection accuracy of the target P.

In the step of calculating the incident angle θ, the angle between the straight line K2 connecting the position AR1 of the moving body 10 and the measurement point M when the second detection is performed and the normal line (perpendicular line K1) of the approximation line is calculated as the incident angle θ. By calculating the incident angle θ in this manner, the calculation accuracy of the incident angle θ is improved, and the decrease in the detection accuracy of the target P can be further effectively suppressed.

After the point group M0 is selected from the second point group, if the third point group corresponding to the third detection of the sensor 26 is acquired, in the step of detecting the approximation line K, the approximation line K is updated and detected based on the first point group and the second point group. In addition, in the step of calculating the incidence angle θ, the incidence angle θ of the light from the sensor 26 to the target P in the third detection is calculated based on the positional relationship between the third point group, the moving body 10 at the time of the third detection, and the updated approximation line K. In addition, in the step of selecting the point group M0, the point group M0 for specifying the position and orientation of the target P is selected from the third point group based on the incidence angle θ in the third detection, and in the step of specifying the position and orientation of the target P, the position and orientation of the target P are specified based on the point group M0 selected from the second point group and the point group M0 selected from the third point group. In this way, by increasing the point group M0 used to calculate the approximation line K each time detection is performed, the reproducibility of the front surface Pa of the target P by the approximation line K is improved, and the deterioration of the detection accuracy of the target P can be further suppressed.

The control method disclosed herein further includes a step of calculating a straight line L corresponding to the front surface Pa of the target P based on the selected point cloud M0, and a step of extracting a point cloud M0 for identifying the position and orientation of the target P from the selected point cloud M0 based on the positional relationship between the selected point cloud M0 and the straight line L. In the step of identifying the position and orientation of the target P, the position and orientation of the target P are identified based on the extracted point cloud M0. In this control method, the point cloud M0 to be used is selected based on the incident angle θ, and further, the point cloud M0 to be used is extracted based on the straight line L calculated from the selected point cloud M0, thereby more preferably excluding the point cloud M0 with low detection accuracy and more preferably suppressing the decrease in the detection accuracy of the target P.

In the step of extracting the point cloud M0, the point cloud M0 that is within a predetermined distance range from the straight line L is extracted as the step of extracting the point cloud M0. According to this control method, by extracting the point cloud M0 used to identify the position and orientation of the target P in this manner, it is possible to more effectively suppress a decrease in the detection accuracy of the target P.

The moving body 10 of the present disclosure moves automatically, and includes a detection control unit 74 that detects the target P multiple times by irradiating the sensor 26 provided on the moving body 10 with light, and acquires the results of the multiple detections by the sensor 26 as a point cloud M0, and a target information acquisition unit 78 that acquires information on the position and attitude of the target P identified based on the point cloud M0. The position and attitude of the target P are identified based on the point cloud M0 selected from the second point cloud by calculating the incident angle θ of the light from the sensor 26 to the target P in the second detection based on the approximation line K of the point cloud M0 detected based on the first point cloud, which is the point cloud M0 corresponding to the first detection of the sensor 26, the second point cloud, which is the point cloud M0 corresponding to the second detection of the sensor 26, and the position AR1 of the moving body 10 when the second detection is performed. Based on the incident angle θ, the point cloud M0 for identifying the position and attitude of the target P is selected from the second point cloud. According to this moving body 10, by acquiring information on the position and orientation of the target P identified using the measurement point M with high detection accuracy, it is possible to effectively suppress a decrease in the detection accuracy of the target P.

The program disclosed herein is a program for making a computer execute a method for controlling a moving body 10 that moves automatically, and causes the computer to execute the following steps: detecting a target P multiple times by irradiating light to a sensor 26 provided on the moving body 10, and acquiring the results of the multiple detections by the sensor 26 as a point cloud M0; detecting an approximation line K of the point cloud M0 based on a first point cloud, which is the point cloud M0 corresponding to the first detection of the sensor 26; calculating an incidence angle θ of light from the sensor 26 to the target P in the second detection based on the approximation line K, a second point cloud, which is the point cloud M0 corresponding to the second detection of the sensor 26, and the position AR1 of the moving body 10 when the second detection was performed; selecting a point cloud M0 for identifying the position and orientation of the target P from the second point cloud based on the incidence angle θ; and identifying the position and orientation of the target P based on the point cloud M0 selected from the second point cloud. This program makes it possible to suitably suppress a decrease in the detection accuracy of the target P.

Although the embodiment of the present invention has been described above, the embodiment is not limited to the contents of this embodiment. The above-mentioned components include those that a person skilled in the art can easily imagine, those that are substantially the same, and those that are within the so-called equivalent range. Furthermore, the above-mentioned components can be combined as appropriate. Furthermore, various omissions, substitutions, or modifications of the components can be made without departing from the spirit of the above-mentioned embodiment.

REFERENCE SIGNS LIST 10 Mobile object 26 Sensor 70 First path information acquisition unit 72 Movement control unit 74 Detection control unit 76 Point cloud superposition unit 78 Target information acquisition unit 80 Second path information acquisition unit K Approximation line M, MA Measurement point M0 Point cloud N Approximation line P Target θ Incident angle

Claims (10)

A method for controlling an automatically moving object, comprising:
a step of detecting a target a plurality of times by irradiating a sensor provided on the moving object with light, and acquiring a result of the plurality of detections by the sensor as a point cloud;
detecting an approximation line of the first point cloud based on a first point cloud corresponding to a first detection of the sensor;
calculating an incident angle of light from the sensor in the second detection to the approximation line based on the approximation line, a second point cloud that is a point cloud corresponding to the second detection of the sensor, and a position of the moving object when the second detection is performed;
selecting a point cloud from the second point cloud based on the incident angle for identifying a position and orientation of the target object;
determining a position and orientation of the target based on a selected set of points from the second set of points;
including,
A method for controlling a moving object. setting an approach path to a target position that has a predetermined position and attitude with respect to the target based on the position and attitude of the target;
The method for controlling a moving body according to claim 1 , further comprising the step of: moving the moving body along the approach path. acquiring information of a wide area path across an installation area in which a target is installed;
and moving the moving object along the wide area path;
3. The method for controlling a moving body according to claim 1, wherein in the step of acquiring the point cloud, the sensor is caused to detect the target object a plurality of times while the moving body is moving along the wide area path. In the step of calculating the incidence angle, the incidence angle is calculated for each measurement point included in the second point group,
4. The method for controlling a moving body according to claim 1, wherein in the step of selecting the point cloud, measurement points included in the second point cloud are selected whose incidence angle is within a predetermined range. The method for controlling a moving body according to claim 4, wherein in the step of calculating the angle of incidence, the angle between a straight line connecting the position of the moving body and the measurement point when the second detection is performed and a normal line to the approximation line is calculated as the angle of incidence. if a third cloud of points corresponding to a third detection of the sensor is obtained after a point cloud is selected from the second cloud of points;
In the step of detecting the approximation line, the approximation line is updated and detected based on the first point group and the second point group;
In the step of calculating the angle of incidence, an angle of incidence of light from the sensor to the target in the third detection is calculated based on a positional relationship between the third point group, the moving body at the time of the third detection, and the updated approximation line;
In the step of selecting the point cloud, a point cloud for identifying a position and an attitude of the target object is selected from the third point cloud based on the incident angle;
6. A method for controlling a moving body according to claim 1, wherein, in the step of specifying the position and attitude of the target object, the position and attitude of the target object are specified based on a point cloud selected from the second point cloud and a point cloud selected from the third point cloud. calculating a straight line corresponding to a front surface of the target object based on the selected points;
extracting a point cloud for identifying a position and an orientation of a target object from the selected point cloud based on the selected point cloud and the straight line;
7. The method for controlling a moving body according to claim 1, wherein in the step of specifying the position and orientation of the target object, the position and orientation of the target object are specified based on the extracted point cloud. In the step of extracting the point cloud,
The method for controlling a moving body according to claim 7 , further comprising the step of extracting the group of points that are within a predetermined distance range from the straight line. A moving object that moves automatically,
a detection control unit that detects a target object a plurality of times by irradiating a sensor provided on the moving object with light and acquires a result of the plurality of detections by the sensor as a point cloud;
a target information acquisition unit that acquires information on the position and attitude of the target identified based on the point cloud,
The position and orientation of the target are
an incident angle of light from the sensor in the second detection to the approximation line is calculated based on an approximation line of the first point cloud detected based on a first point cloud corresponding to a first detection of the sensor, a second point cloud corresponding to a second detection of the sensor, and a position of the moving body when the second detection is performed;
A point cloud for identifying the position and the attitude of the target is selected from the second point cloud based on the incident angle,
is determined based on a set of points selected from the second set of points.
Mobile body. A program for causing a computer to execute a method for controlling an automatically moving object,
a step of detecting a target a plurality of times by irradiating a sensor provided on the moving object with light, and acquiring a result of the plurality of detections by the sensor as a point cloud;
detecting an approximation line of the first point cloud based on a first point cloud corresponding to a first detection of the sensor;
calculating an incident angle of light from the sensor in the second detection to the approximation line based on the approximation line, a second point cloud that is a point cloud corresponding to the second detection of the sensor, and a position of the moving object when the second detection is performed;
selecting a point cloud from the second point cloud based on the incident angle for identifying a position and an orientation of the target object;
determining a position and orientation of the target based on a selected set of points from the second set of points;
The computer executes the
program.

Images