Control device, mobile body, marker, position estimation method, and program

Abstract

The present invention suitably estimates the position of a mobile body. This control device includes: a map acquisition unit that acquires map information of a target region in which the mobile body moves; a detection control unit that acquires detection results from detecting the surrounding environment, said detection being carried out by using a sensor mounted on the mobile body; a first computation unit that computes a first estimated position of the mobile body in the target region on the basis of a marker detection result obtained by detecting a marker from among the detection results, and on the basis of position information of the marker in the target region; and a second computation unit that computes a second estimated position of the mobile body in the target region by performing an optimization calculation on the basis of the map information and the detection results, the first estimated position being used as the initial solution in the optimization calculation.

Description

Control device, mobile object, sign, position estimation method, and program

[0001] The present disclosure relates to a control device, a moving object, a sign, a position estimation method, and a program.

[0002] A mobile object that moves while estimating its own position is known. For example, Patent Document 1 describes that a local map of the mobile object's surroundings created from reflected waves of laser light emitted from a laser range finder is compared with an environmental map to estimate its own position. Patent Document 1 also describes that a sign is detected from an output pattern of reflected waves of laser light, the position of the detected sign on the environmental map is identified based on the estimated self-position, and the estimated self-position is corrected based on the identified position of the sign on the environmental map.

[0003] Patent No. 5983088

[0004] However, in Patent Document 1, the position of the sign on the environmental map is identified from the estimated self-location obtained by comparing the local map with the environmental map, and the estimated self-location is corrected. Therefore, if the estimation accuracy of the estimated self-location is low, the position of the sign on the environmental map may not be identified appropriately, and as a result, the self-location may not be estimated appropriately. Therefore, there is a need to appropriately estimate the position of the moving object.

[0005] The present disclosure aims to provide a control device, a moving body, a sign, a position estimation method, and a program that are capable of appropriately estimating the position of a moving body.

[0006] The control device according to the present disclosure includes a map acquisition unit that acquires map information of a target area in which a moving body moves; a detection control unit that acquires detection results of the surrounding environment by a sensor mounted on the moving body; a first calculation unit that calculates a first estimated position of the moving body in the target area based on a sign detection result that detects a sign from the detection results and position information of the sign in the target area; and a second calculation unit that performs optimization calculations based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the moving body in the target area.

[0007] A moving body according to the present disclosure includes the control device.

[0008] The sign of the present disclosure is a sign used for estimating the self-position of a moving body, and has multiple reflective elements, at least some of which have different reflectivities, and at least one of the number and order of the reflective elements is set to a pattern that makes it distinguishable from other signs.

[0009] The position estimation method according to the present disclosure includes the steps of acquiring map information of a target area in which a moving body is moving, acquiring detection results of the surrounding environment by a sensor mounted on the moving body, calculating a first estimated position of the moving body in the target area based on a sign detection result that detects a sign among the detection results and position information of the sign in the target area, and performing an optimization calculation based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the moving body in the target area.

[0010] The program of the present disclosure causes a computer to execute the following steps: acquiring map information of a target area in which a moving body is moving; acquiring detection results of the surrounding environment by a sensor mounted on the moving body; calculating a first estimated position of the moving body in the target area based on a sign detection result that detects a sign from the detection results and position information of the sign in the target area; and performing an optimization calculation based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the moving body in the target area.

[0011] According to the present disclosure, the position of a moving body can be appropriately estimated.

[0012] FIG. 1 is a schematic diagram illustrating a moving body according to a first embodiment. FIG. 2 is a schematic diagram illustrating an example of a sign according to this embodiment. FIG. 3 is a schematic diagram illustrating an example of a sign according to this embodiment. FIG. 4 is a schematic diagram of the configuration of a moving body. FIG. 5 is a schematic block diagram of a control device of a moving body. FIG. 6 is a schematic diagram illustrating a method of extracting a sign detection result. FIG. 7 is a schematic diagram illustrating an example of a method of identifying a sign. FIG. 8 is a schematic diagram illustrating a method of calculating a second estimated position. FIG. 9 is a flowchart illustrating a processing flow for setting a self-position. FIG. 10 is a schematic diagram illustrating an example of a process for adding a detection result to map information.

[0013] Preferred embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. Note that the present disclosure is not limited to these embodiments, and when there are multiple embodiments, the present disclosure also includes configurations in which the respective embodiments are combined.

[0014] First Embodiment FIG. 1 is a schematic diagram illustrating a mobile body according to a first embodiment. In this embodiment, the mobile body 10 belongs to a facility F and moves within the facility F. The facility F is, for example, a warehouse or other facility subject to logistics management. However, the facility F may be any facility that operates the mobile body 10, and may be indoors or outdoors. The mobile body 10 picks up and transports an object located within a target area AR of the facility F. The target area AR is an area where objects are installed and where the mobile body 10 moves, such as the floor of the facility F. In this embodiment, the object transported by the mobile body 10 is a transport object in the form of a load loaded on a pallet. However, the object is not limited to a load loaded on a pallet and may be in any form, for example, it may be only a load without a pallet. Furthermore, the mobile body 10 is not limited to a device that transports objects, and may be a device that moves within the facility F for any purpose.

[0015] (Area) An installation W is installed within the target area AR. The installation W refers to an object that is placed within the target area AR depending on the purpose of the facility F. The installation position of the installation W is determined in advance when the facility F is designed or when the facility F starts operation, and it is preferable that the installation W does not move from the determined position. For example, the installation W may be a wall, a pillar, a shelf, etc., and the installation W in this embodiment does not refer to a moving object such as a person, a moving object, or an object to be transported. In the example of FIG. 1 , the installation W includes an installation W1 that is a pillar and an installation W2 that is a wall, but the arrangement of the installations W in the target area AR, in other words, the layout of the target area AR, is set as appropriate.

[0016] Hereinafter, one direction along the target area AR is referred to as the X direction, and a direction along the target area AR that intersects with the X direction is referred to as the Y direction. In this embodiment, the Y direction is a direction perpendicular to the X direction. The X and Y directions may also be referred to as directions along a horizontal plane. Furthermore, the direction perpendicular to the X and Y directions, more specifically, the direction pointing vertically upward, is referred to as the Z direction. Furthermore, in this embodiment, unless otherwise specified, "position" refers to a position (coordinate) in a coordinate system on a two-dimensional surface on the target area AR (the coordinate system of the target area AR). Furthermore, unless otherwise specified, "attitude (orientation)" of the moving body 10 or the like refers to the orientation of the moving body 10 or the like in the coordinate system of the target area AR, and refers to the yaw angle (rotation angle) of the moving body 10 when the X direction is set to 0° when viewed from the Z direction.

[0017] (Signs) Signs 100 are placed in the target area AR (facility F). The sign 100 refers to an object that serves as a landmark for estimating the self-position of the mobile body 10. The sign 100 is an object that can be detected by a sensor 26A of the mobile body 10, which will be described later, and may have, for example, a reflective member that can reflect laser light from the sensor 26A. It is preferable that the positions at which the signs 100 are installed are determined in advance when the facility F is designed or when the facility F starts operating, and that the signs 100 do not move from the determined positions. In the example of FIG. 1 , two signs 100 are installed on the installation W1, which is a pillar, and one sign 100 is installed on the installation W2, which is a wall, but the installation positions and number of signs 100 can be set appropriately. It is preferable that the signs 100 are installed so that the distance between each other is equal to or greater than a predetermined distance.

[0018] 2 is a schematic diagram showing an example of a sign according to this embodiment. The sign 100 may be any component detectable by the moving body 10, but in this embodiment, the sign 100 is configured to be able to identify which sign 100 is present among the signs 100 installed in the target area AR (i.e., be distinguishable from other signs) based on the detection results of the sensor 26A of the moving body 10. The configuration of the sign 100 according to this embodiment will be described below. Note that the reflectance here refers to the reflectance of light irradiated from the sensor 26A.

[0019] The sign 100 has a plurality of reflective members 110, at least some of which have different reflectivities. As shown in FIG. 2 , in this embodiment, the sign 100 has reflective members 110A and 110B as the reflective members 110. The reflective member 110B has a higher reflectivity than the reflective member 110A. In this embodiment, the reflective member 110B is a plate-shaped member that is rectangular when viewed from the normal direction of the surface of the sign 100. However, the shape of the reflective member 110B is not limited to this and may be any shape. The reflective member 110B may be made of any material, such as a mirror material. The reflective member 110A has a lower reflectivity than the reflective member 110B. In this embodiment, the reflective member 110A has a rectangular shape when viewed from a direction perpendicular to the surface of the sign 100. However, the shape of the reflective member 110A is not limited to this and may be any shape. The reflective member 110A may be made of any material, such as a plate-shaped light-absorbing material. Furthermore, the reflective member 110A may be the surface of the installation W to which the sign 100 is attached. In other words, in this case, the sign 100 is divided into an area where the reflective member 110B is provided and an area where the surface of the installation W is exposed, and the area where the surface of the installation W is exposed can be said to constitute the reflective member 110A. In this embodiment, when viewed from the normal direction to the surface of the sign 100, the reflective members 110A and 110B are the same in size and shape in all signs 100. Furthermore, in this embodiment, when viewed from the normal direction to the surface of the sign 100, the size and shape of the reflection of the reflective members 110A and 110B are the same in all signs 100. However, this is not limited thereto, and the size and shape of the reflective members 110A and 110B may differ for each sign 100. Furthermore, the sign 100 is not limited to having only two types of reflective members 110A and 110B with different reflectances, but may have a configuration with three or more types of reflective members with different reflectances.

[0020] In the sign 100, the reflective members 110 (in this example, reflective members 110A and 110B) are aligned in a direction (horizontal direction) perpendicular to the Z direction. In the example of Fig. 2, the reflective members 110 are provided on the surface of the installation W, the normal direction of which is the Y direction, and therefore aligned in the X direction. It can be said that the reflective members 110 are aligned in a direction perpendicular to the Z direction and perpendicular to the normal direction of the surface of the installation W.

[0021] The signs 100 have a pattern in which at least one of the number and arrangement order of the reflective members 110 makes it possible to identify which sign 100 it is, i.e., a pattern that makes it possible to distinguish the signs 100 from other signs. In other words, the signs 100 installed in the target area AR (facility F) differ from one another in at least one of the number and arrangement order of the reflective members 110. Therefore, when the sensor 26A of the mobile body 10 detects each sign 100, the feature quantities of the detection results differ from one another. As a result, the mobile body 10 can identify which sign 100 the detected sign 100 is by detecting the sign 100. An example of this identification method will be described later.

[0022] In the signs 100, at least one of the number and arrangement order of the reflective members 110 may be set in any pattern that allows identification of each sign 100. For example, in this embodiment, all signs 100 have reflective members 110A with low reflectivity arranged at both ends in the direction in which the reflective members 110 are arranged (the X direction in FIG. 2 ). Then, between the two reflective members 110A at both ends, reflective members 110A or reflective members 110B are arranged as reflective members 110. The number of reflective members 110 between the two reflective members 110A at both ends may be arbitrary. For example, in this embodiment, the number of reflective members 110 between the two reflective members 110A at both ends is the same in all signs 100. In FIG. 2 , the number of reflective members 110 between the two reflective members 110A at both ends is illustrated as seven. In this embodiment, the arrangement order of the reflective members 110A and 110B between the two reflective members 110A at both ends varies for each sign 100. For example, in the example of Fig. 2, the arrangement between the two reflective members 110A at both ends is, from the left, reflective member 110B, reflective member 110B, reflective member 110A, reflective member 110B, reflective member 110B, reflective member 110A, and reflective member 110B, but other signs 100 do not have this arrangement.

[0023] FIG. 3 is a schematic diagram showing an example of a sign according to this embodiment. The sign 100 may be provided at any position on the surface of an installation W. For example, as shown in FIG. 3 , when the sign 100 is placed on an installation W1 that is a pillar, the sign 100 may be provided on multiple surfaces of the installation W1. In the example of FIG. 3 , a sign 100 is provided on each of two side surfaces of the installation W1. In this case, the signs 100 provided on the same installation W1 may have different patterns of reflective members 110, or may have the same pattern. If the reflective members 110 have the same pattern, it is preferable that the positions in the Z direction of the signs 100 provided on different surfaces are different. This makes it possible to identify which sign 100 has been detected based on the position in the Z direction at which the sign 100 was detected.

[0024] The position of the marker 100 configured as above in the coordinate system of the target area AR is known in advance. That is, the correspondence between the marker 100 and the position information of the marker 100 is set in advance for each marker 100.

[0025] (Mobile body) FIG. 4 is a schematic diagram of the configuration of a mobile body. The mobile body 10 is a device capable of moving automatically. In this embodiment, the mobile body 10 is a non-holonomic system that cannot move sideways, but is not limited to this. In this embodiment, the mobile body 10 is a device capable of transporting a target object. Furthermore, in this embodiment, the mobile body 10 is a forklift, and more specifically, a so-called AGV (Automated Guided Vehicle) or AGF (Automated Guided Forklift). However, the mobile body 10 is not limited to a forklift that transports a target object, and may be any device capable of moving automatically.

[0026] As shown in FIG. 2 , the vehicle 10 includes a vehicle body 20, wheels 20A, straddle legs 21, a mast 22, a fork 24, a sensor 26A, an internal sensor 26B, and a control device 28. The straddle legs 21 are a pair of shaft-shaped members provided at one end of the vehicle body 20 in the longitudinal direction and protruding from the vehicle body 20. The wheels 20A are provided at the tip of each straddle leg 21 and on the vehicle body 20. That is, a total of three wheels 20A are provided, but the positions and number of the wheels 20A may be arbitrary. The mast 22 is movably attached to the straddle legs 21 and moves in the longitudinal direction of the vehicle body 20. The mast 22 extends in an up-down direction (direction Z in this case) perpendicular to the longitudinal direction. The fork 24 is attached to the mast 22 movably in direction Z. The fork 24 may also be movable in the lateral direction of the vehicle body 20 (a direction intersecting the up-down and front-rear directions) 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, in the front-rear direction, the direction on the side of the vehicle 10 where the fork 24 is not provided will be referred to as the forward direction, and the direction on the side where the fork 24 is provided will be referred to as the rearward direction.

[0027] The sensor 26A detects at least one of the position and posture of an object (such as an installed object W or an object other than the installed object W) present around the vehicle body 20. It can also be said that the sensor 26A detects at least one of the position of the object relative to the mobile body 10 and the posture of the object relative to the mobile body 10. In this embodiment, the sensor 26A is provided at the front end of each straddle leg 21 and on the rear side of the vehicle body 20. However, the positions at which the sensor 26A is provided are not limited thereto, and the sensor 26A may be provided at any position, and the number of sensors provided may also be arbitrary.

[0028] The sensor 26A is, for example, a sensor that emits laser light. The sensor 26A emits laser light while scanning in one direction (here, the horizontal direction), and detects the position and orientation of an object from the reflected light of the emitted laser light. In other words, the sensor 26A can also be said to be a so-called two-dimensional (2D)-LiDAR (Light Detection and Ranging). However, the sensor 26A is not limited to the above and may be a sensor that detects an object by any method. For example, the sensor 26A may be a so-called three-dimensional (3D)-LiDAR that scans in multiple directions, a so-called one-dimensional (1D)-LiDAR that does not scan, or a camera.

[0029] The internal sensor 26B is mounted on the mobile body 10 and detects output parameters of the mobile body 10. The output parameters are parameters that indicate the control output of the mobile body 10. More specifically, the output parameters indicate the degree to which the mobile body 10 operates under control, and can be considered to be output values ​​from the mobile body 10 when control input is provided to the mobile body 10. That is, the control device 28 provides control input to the drive mechanism (such as an actuator) of the mobile body 10 so that the mobile body 10 moves in a desired direction and at a desired speed. The drive mechanism of the mobile body 10 operates in response to the control input, and the output parameters corresponding to the control input are detected by the internal sensor 26B. The output parameters may be any parameters that indicate the control output of the mobile body, but in this embodiment, they are the rotation speed of the wheels 20A of the mobile body 10 and the steering angle of the mobile body 10. In this case, the internal sensor 26B detects the rotation speed of the wheels 20A (the number of rotations per unit time) and the steering angle of the mobile body 10 (the amount of change in the direction of the wheels 20A). The internal sensor 26B may include a sensor for detecting the rotation speed and a sensor for detecting the steering angle, or a single sensor may be used to detect both the rotation speed and the steering angle. The internal sensor 26B may be, for example, an IMU (Inertial Measurement Unit).

[0030] The control device 28 controls the movement of the moving body 10. The configuration of the control device 28 will be described below.

[0031] (Control Device) FIG. 5 is a schematic block diagram of a control device of a mobile body. The control device 28 is a device that controls the mobile body 10. The control device 28 is a computer, and as shown in FIG. 5, includes a communication unit 30, a storage unit 32, and a control unit 34. The communication unit 30 is a module used by the control unit 34 to communicate with external devices, and may include, for example, an antenna. In this embodiment, the communication method used by the communication unit 30 is wireless communication, but any communication method may be used. The storage unit 32 is a memory that stores various information such as the calculation contents and programs of the control unit 34, 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.

[0032] The control unit 34 is a computing device and includes a computing circuit such as a CPU. The control unit 34 includes a map acquisition unit 40, a detection control unit 42, an extraction unit 44, a sign acquisition unit 46, a first calculation unit 48, a second calculation unit 50, and a movement control unit 52. The control unit 34 reads and executes a program (software) from the storage unit 32, thereby realizing the map acquisition unit 40, the detection control unit 42, the extraction unit 44, the sign acquisition unit 46, the first calculation unit 48, the second calculation unit 50, and the movement control unit 52 and performing their processing. The control unit 34 may perform these processes using a single CPU, or may be provided with multiple CPUs and perform the processes using the multiple CPUs. Furthermore, at least a portion of the map acquisition unit 40, the detection control unit 42, the extraction unit 44, the sign acquisition unit 46, the first calculation unit 48, the second calculation unit 50, and the movement control unit 52 may be realized by hardware circuits. Furthermore, the program for the control unit 34 stored in the storage unit 32 may be stored in a recording medium that can be read by the control device 28 .

[0033] (Processing of the Control Device) Next, the processing content of the control device 28 will be described.

[0034] (Movement Control of Mobile Body) The movement control unit 52 of the control device 28 controls the movement of the mobile body 10 by controlling the movement mechanisms such as the drive unit and steering of the mobile body 10. The movement control unit 52 moves the mobile body 10 so as to follow the set route R (e.g., FIG. 1 ) of the mobile body 10. In this case, the control device 28 successively estimates the position and attitude (self-position) of the mobile body 10 and moves the mobile body 10 so that the self-position at each timing approaches the route R. Therefore, if the deviation between the estimated self-position and the actual self-position becomes large, the deviation amount between the actual movement route and the route R may also become large. In contrast, in this embodiment, the control device 28 uses the self-position (first estimated position described later) estimated from the detection result of the sign 100 by the sensor 26A (sign detection result) as an initial solution, and estimates the self-position of the mobile body 10 (second estimated position described later) by performing an optimization calculation using the detection result of the surrounding environment by the sensor 26A and map information of the target area AR. By estimating the self-location in this manner, the position of the moving body 10 can be appropriately estimated, and the deviation between the estimated self-location result and the actual self-location can be prevented from becoming too large. The self-location estimation method will be specifically described below.

[0035] (Acquisition of Map Information) The map acquisition unit 40 acquires map information of the target area AR. The map information of the target area AR is information including position information of installed objects W installed in the target area AR. The map information of the target area AR can also be said to be information indicating the positions of the installed objects W in the coordinate system of the target area AR. In this embodiment, the map information of the target area AR is data that represents the positions (coordinates) of the installed objects W as a point cloud, i.e., a point cloud map. The map acquisition unit 40 may acquire the map information of the target area AR using any method. For example, the map acquisition unit 40 may acquire this information from another device or the like via the communication unit 30, or the map information may be stored in the memory unit 32 and the map acquisition unit 40 may read the map information from the memory unit 32.

[0036] (Detection of Surrounding Environment) The detection control unit 42 controls the sensor 26A to cause the sensor 26A to detect the surrounding environment. The detection control unit 42 acquires the detection result of the surrounding environment by the sensor 26A. The detection control unit 42 causes the sensor 26A to detect the surroundings at predetermined intervals, and acquires the detection result of the surrounding environment at predetermined intervals.

[0037] For example, if the sensor 26A is configured to emit laser light, the detection control unit 42 causes the sensor 26A to emit laser light while scanning the sensor 26A in the horizontal direction. Objects such as an installation W around the sensor 26A reflect the laser light from the sensor 26A, and the sensor 26A receives the reflected light. The detection control unit 42 calculates the position of the location where the reflected light is reflected in the coordinate system of the mobile body 10 (sensor 26A) from the reflected light received by the sensor 26A. The detection control unit 42 acquires, as the detection result of the sensor 26A, a point cloud indicating the position (measurement point) of the location where the reflected light is reflected in the coordinate system of the mobile body 10 (sensor 26A). That is, the detection control unit 42 acquires, as the detection result of the sensor 26A, data including a point cloud of each location where the reflected light is reflected. Note that the point cloud includes information on the location where the reflected light is reflected (in this example, the position in the coordinate system of the mobile body 10) and information on the intensity of the reflected light. Hereinafter, information on the intensity of reflected light contained in a point cloud will be referred to as point cloud intensity.

[0038] Furthermore, in this embodiment, the detection control unit 42 controls the internal sensor 26B to cause the internal sensor 26B to detect the output parameters of the moving body 10. The detection control unit 42 acquires the detection results of the output parameters of the moving body 10 by the internal sensor 26B. The detection control unit 42 acquires the detection results of the output parameters at predetermined intervals.

[0039] (Extraction of Sign Detection Results) The extraction unit 44 extracts sign detection results, which are detection results of detecting the sign 100, from the detection results of the surrounding environment by the sensor 26A. That is, if the sign 100 is present within the detectable range of the sensor 26A, the detection results of the surrounding environment acquired by the detection control unit 42 will include the sign detection result of detecting the sign 100. If the sensor 26A has detected the sign 100, that is, if the detection results of the surrounding environment include the sign detection result, the extraction unit 44 selects and extracts the sign detection result from the detection results of the surrounding environment.

[0040] The extraction unit 44 may extract the sign detection result using any method. In this embodiment, the extraction unit 44 extracts, as the sign detection result, a detection result having a predetermined feature value from among the detection results of the surrounding environment. If the detection results of the surrounding environment do not include a detection result having the predetermined feature value, the extraction unit 44 determines that the sign 100 is not identified and does not extract a sign detection result. In this embodiment, the point cloud intensity (intensity of reflected light) is used as the feature value of the detection result. In this case, for example, a predetermined point cloud intensity pattern is set in advance, and the extraction unit 44 may extract, as the sign detection result, a point cloud having the predetermined point cloud intensity pattern from among the point clouds included in the detection results of the surrounding environment. The predetermined point cloud intensity pattern may be set arbitrarily so as to be a pattern that is expected to be obtained when the sign 100 is irradiated with laser light. An example of this embodiment will be described below.

[0041] FIG. 6 is a schematic diagram illustrating a method for extracting a sign detection result. FIG. 6 shows an example of a point cloud obtained when a laser beam is irradiated onto the sign 100 shown in FIG. 2 . In this embodiment, the extraction unit 44 extracts, as point cloud A1, point clouds A having a point cloud intensity equal to or greater than a predetermined threshold from among point clouds A arranged horizontally obtained by a single scan by the sensor 26A. The threshold may be set arbitrarily, for example, so that the intensity of reflected light from the reflecting member 110B is equal to or greater than the threshold and the intensity of reflected light from the reflecting member 110A and other installed objects W is less than the threshold. Among the horizontally arranged point clouds A, point cloud A1 located closest to one end in the horizontal direction (left side in the example of FIG. 6 ) is designated point cloud A1a. Furthermore, among point clouds A1 located within a predetermined distance D from point cloud A1a on the other end in the horizontal direction (right side in the example of FIG. 6 ), point cloud A1 located closest to the other end is designated point cloud A1b. The predetermined distance D may be set as appropriate, for example, to be equal to or greater than the horizontal length of the marker 100. In this case, the extraction unit 44 extracts each point group A (scan line) from point group A1a to point group A1b among the point groups A arranged horizontally as a marker detection result (a point group where the marker 100 is detected). In FIG. 6 , point group A1, whose point group intensity is equal to or greater than a threshold, and point group A0, whose point group intensity is less than the threshold, are included between point group A1a and point group A1b, and both of these are extracted as a marker detection result (a point group where the marker 100 is detected). Note that if there is no point group A1 whose point group intensity is equal to or greater than a predetermined threshold, the marker 100 is not identified, and no marker detection result is extracted. Furthermore, for example, if point group A1 is located at a position closer to the other end (the right side in the example of FIG. 6 ) than the predetermined distance D than point group A1a, that point group A1 is set as point group A1a on the horizontally most end side, and it is determined whether there is another marker detection result.

[0042] For example, if multiple scans are performed by the sensor 26A, resulting in multiple sets of point groups A (scan lines) from point group A1a to point group A1b, the extraction unit 44 may select one of these sets as the sign detection result. In this case, for example, the extraction unit 44 may not use (exclude) as the sign detection result any scan lines containing fewer than a predetermined number of point groups A, any scan lines whose distance from the sensor 26A to the point groups A is outside a predetermined range (i.e., scan lines that are too close or too far from the sensor 26A), or any scan lines whose length from point group A1a to point group A1b is outside a predetermined range (i.e., scan lines that are too long or too short). Furthermore, Euclidean clustering may be performed at the position of the starting point (point group A1a), and the scan line whose starting point belongs to the largest cluster may be retained as a candidate for the sign detection result. If multiple sets of scan lines remain that have not been excluded, the extraction unit 44 may select the scan line with the largest number of point groups A as the sign detection result.

[0043] In this embodiment, the extraction unit 44 preferably sets a region of interest from which to extract sign detection results based on the self-position calculated in the past (preferably immediately before). The extraction unit 44 then selects detection results (point clouds) located within the region of interest from among the detection results of the surrounding environment, and extracts sign detection results from the selected detection results. By setting a region of interest and extracting sign detection results in this way, the number of point clouds to be searched for sign detection results can be reduced, thereby reducing the computational load.

[0044] The extraction unit 44 may set the region of interest using any method. For example, the extraction unit 44 may set the region of interest based on the most recently calculated self-position and the detection results (detection results of output parameters) of the internal sensor 26B after the most recently calculated self-position. In this case, for example, the extraction unit 44 calculates the movement direction and movement amount of the moving object 10 from the most recently calculated self-position based on the detection results of the internal sensor 26B, and calculates the position and posture of the moving object 10 moved by the movement direction and movement amount from the most recently calculated self-position as the current estimated position of the moving object 10. Then, the extraction unit 44 may set, as the region of interest, an area including the position of the marker 100 relative to the calculated estimated position.

[0045] (Identification of Sign) The sign acquisition unit 46 identifies the sign 100 detected by the sensor 26A based on the feature amount of the sign detection result extracted by the extraction unit 44, and acquires position information of the identified sign 100. That is, the sign acquisition unit 46 identifies which of the signs 100 in the target area AR is the sign 100 currently detected by the sensor 26A, based on the feature amount of the sign detection result.

[0046] The sign acquisition unit 46 may identify the sign 100 detected by the sensor 26A using any method based on the feature amount of the sign detection result. However, in this embodiment, the sign 100 is identified based on the point cloud intensity (intensity of reflected light) of the point cloud included in the sign detection result and the number of points. As described above, the sign 100 has a pattern in which at least one of the number and arrangement order of the reflective members 110 allows the sign 100 to be identified. In other words, the point cloud intensity pattern of the sign detection result differs for each sign 100. The sign acquisition unit 46 uses this to identify, among each sign 100, the sign 100 that matches the point cloud intensity pattern indicated by the sign detection result as the sign 100 detected by the sensor 26A. Note that if there is no sign 100 that matches the point cloud intensity pattern indicated by the sign detection result, the sign acquisition unit 46 determines that the sign 100 cannot be identified and does not identify the sign 100.

[0047] The method for identifying the marker 100 according to this embodiment will be described in more detail below. FIG. 7 is a schematic diagram illustrating an example of the method for identifying the marker. As in step S1 of FIG. 7 , the marker acquisition unit 46 divides the section from point group A1a to point group A1b included in the marker detection result into a predetermined number of sections C in the direction from point group A1a to point group A1b. The number of sections C here is the same as the number of reflective members 110 between the two reflective members 110A at both ends of the marker 100, and the lengths of the sections are the same. In the example of FIG. 7 , the section from point group A1a to point group A1b is divided into seven sections, C1 to C7, in order from the point group A1a side (the left side in FIG. 7 ).

[0048] Then, as in step S2 of FIG. 7 , the marker acquisition unit 46 calculates an evaluation index E for each section C from the number of point groups A1 and the number of point groups A0 included in each section C. The marker acquisition unit 46 calculates the evaluation index E so that the greater the number of point groups A1 included in section C, the higher the value, and the greater the number of point groups A0 included in section C, the lower the value. In the example of FIG. 7 , the initial value is 0, and the evaluation index E is calculated by adding the number of point groups A1 included in section C and subtracting the number of point groups A1 included in section C. For example, in section C2 of FIG. 7 , there are two point groups A1 and one point group A0, so the evaluation index E is 0 + 2 - 1, or 1. Note that if there is a section C where the absolute value of the evaluation index E is less than a predetermined threshold, it is determined that the marker 100 cannot be identified, and the marker 100 is not identified.

[0049] 7, the marker acquisition unit 46 normalizes the evaluation index E for each section C to calculate an evaluation index Ea for each section C. In the example of Fig. 7, the marker acquisition unit 46 sets a positive evaluation index E to 1, and a negative evaluation index E to 0.

[0050] The sign acquisition unit 46 then identifies the sign 100 detected by the sensor 26A based on the evaluation index Ea for each section C. In this case, a correspondence between the sign 100 and the correct value of the evaluation index Ea for each section is set in advance for each sign 100. The sign acquisition unit 46 acquires this correspondence, extracts the correct value of the evaluation index Ea that matches the calculated evaluation index Ea for each section C in the correspondence, and identifies the sign 100 associated with this correct value as the detected sign 100.

[0051] Note that the sign acquisition unit 46 is not limited to identifying the sign 100 by the method described above. For example, in the above description, the evaluation index Ea for each section C is treated as a point cloud intensity pattern, and the sign 100 having a pattern matching the detected evaluation index Ea is identified as the detected sign 100. However, an index other than the evaluation index Ea may be used as the pattern of point cloud intensity information. For example, machine learning may be performed to determine the correspondence between the sign 100 and the point cloud intensity pattern, and the detected point cloud intensity pattern may be input into the machine-learned AI model to obtain the resulting sign 100 (the label with the highest matching probability output from the AI ​​model). Furthermore, for example, if the sensor 26A is a camera, the sign 100 captured by the sensor 26A may be identified by image analysis.

[0052] (Acquisition of position information of sign) The sign acquisition unit 46 acquires position information of the identified sign 100. Here, the position information of the sign 100 refers to the position of the sign 100 in the coordinate system of the target area AR. In this embodiment, a correspondence relationship between the sign 100 and the position information of the sign 100 is set in advance for each sign 100. The sign acquisition unit 46 acquires this correspondence relationship and acquires the position information associated with the identified sign 100 in this correspondence relationship as the position information of the identified sign 100. Note that if the sign acquisition unit 46 determines that the sign 100 cannot be identified, it does not acquire the position information of the sign 100.

[0053] (Calculation of First Estimated Position) The first calculation unit 48 calculates a first estimated position, which is an estimated position of the moving body 10 in the coordinate system of the target area AR, based on the sign detection result and position information of the sign 100 (the position of the sign 100 in the coordinate system of the target area AR). The first estimated position can also be said to be an estimated position and estimated attitude of the moving body 10 in the coordinate system of the target area AR, calculated based on the sign detection result and the position information of the sign 100 without using map information.

[0054] The first calculation unit 48 calculates the relative position of the moving object 10 with respect to the marker 100 (the position of the moving object 10 in the coordinate system of the marker 100) based on the marker detection result, and calculates a first estimated position from the relative position of the moving object 10 and the position information of the marker 100. That is, since the marker detection result indicates the position of the marker 100 with respect to the moving object 10, the position of the moving object 10 in the coordinate system of the marker 100 can be calculated based on this. For example, in this embodiment, the first calculation unit 48 sets the midpoint position between the point group A1a and the point group A1b included in the marker detection result as the origin position of the coordinate system of the marker 100. Then, the unit vector in the Z direction at the origin position is set to the direction of the marker Z axis, the cross product of the Z axis and the scan line as the normal line is set to the direction of the Y axis, and the cross product of the Z axis and the Y axis is set to the direction of the X axis. The first calculation unit 48 then calculates the position and orientation of the moving body 10 (sensor 26A) in this coordinate system of the X-axis, Y-axis, and Z-axis as the position of the moving body 10 in the coordinate system of the marker 100.

[0055] Then, the first calculation unit 48 performs coordinate transformation of the position of the moving object 10 in the coordinate system of the sign 100 based on the position of the sign 100 in the coordinate system of the target area AR, and calculates a first estimated position of the moving object 10 in the coordinate system of the target area AR. In this way, the first estimated position is not calculated by comparing the map information with the detection result, but is calculated using the sign detection result but without using map information.

[0056] If the sign acquisition unit 46 determines that the sign 100 cannot be identified, the first calculation unit 48 does not calculate the first estimated position.

[0057] (Calculation of Second Estimated Position) The second calculation unit 50 performs optimization calculations based on the map information and the detection results, using the first estimated position calculated by the first calculation unit 48 as an initial solution, to calculate a second estimated position, which is an estimated position of the mobile body 10 in the coordinate system of the target area AR. The second estimated position can also be said to be the estimated position and estimated orientation of the mobile body 10 in the coordinate system of the target area AR, calculated by optimization calculations based on the map information and the detection results, using the first estimated position as an initial solution. Note that the detection results used in the optimization calculations here include the sign detection results used to detect the sign 100 and detection results detecting objects other than the sign 100 (e.g., surrounding installed objects W), and can be said to be all acquired detection results (all acquired point clouds). However, the detection results used in the optimization calculations are not limited thereto, and may include at least detection results detecting objects other than the sign 100.

[0058] The second calculation unit 50 performs an optimization calculation using the first estimated position as an initial solution to find an optimal solution for the position of the moving body in which the deviation between the map information and the detection result is optimized (for example, the deviation amount is equal to or less than a predetermined value), and sets the optimal solution as the second estimated position. The initial solution here refers to a value given as a tentative solution in the first calculation in repeated optimization calculations. That is, the second calculation unit 50 first calculates the deviation amount between the map information and the detection result when the position and orientation of the moving body 10 are at the first estimated position, and then performs an optimization calculation to repeatedly update the position and orientation of the moving body 10 so as to reduce the deviation amount. The second calculation unit 50 calculates the position and orientation of the moving body 10 when the deviation amount has converged (converged solution) as the second estimated position (optimal solution).

[0059] FIG. 8 is a schematic diagram illustrating a method for calculating the second estimated position. In this embodiment, the map information and the detection results are data composed of point clouds. Therefore, as shown in FIG. 8 , the second calculation unit 50 calculates, as the second estimated position, the position and orientation of the mobile object 10 that optimizes the amount of deviation between the point cloud A included in the detection results and a portion of the point cloud Aa included in the map information. Note that the optimization calculation may use, for example, a point cloud registration method (NDT; Normal Distributions Transform). Note that when the sensor 26A is a camera, the first estimated position may be used as an initial solution, and the second estimated position may be calculated by performing an optimization calculation using the image feature amounts of the map information and the image feature amounts of the detection results.

[0060] (Determination of Self-Position) The movement control unit 52 sets the second estimated position calculated by the second calculation unit 50 as the self-position of the moving body 10 (the current position and attitude of the moving body 10 in the coordinate system of the target area AR). The movement control unit 52 moves the moving body 10 based on the set self-position. That is, in this case, movement control of the moving body 10 is performed using the second estimated position that has been set as the self-position.

[0061] (Processing when optimization calculation is performed without using first estimated position) However, if the first estimated position has not been calculated, that is, if the sign acquisition unit 46 determines that the sign 100 cannot be identified, the second calculation unit 50 does not perform optimization calculation using the first estimated position as the initial solution. In this case, the second calculation unit 50 performs optimization calculation to calculate the second estimated position using, as the initial solution, a third estimated position obtained using a method other than the method using the position of the sign 100. The optimization calculation method in this case is the same as the method described above, except that the third estimated position is used as the initial solution.

[0062] In this case, the second calculation unit 50 calculates a third estimated position, which is an estimated position and estimated attitude of the moving body 10 in the coordinate system of the target area AR, based on the self-position calculated in the past (preferably immediately before). For example, in this embodiment, the second calculation unit 50 may calculate the third estimated position based on the self-position calculated in the past (preferably immediately before) and the detection result (detection result of the output parameter) of the internal sensor 26B after the self-position was calculated in the past (preferably immediately before). In this case, for example, the extraction unit 44 calculates the movement direction and movement amount of the moving body 10 from the self-position calculated immediately before based on the detection result of the internal sensor 26B, and calculates the position and attitude moved by that movement direction and movement amount from the self-position calculated immediately before as the third estimated position.

[0063] Furthermore, if the number of times that the second calculation unit 50 has been able to calculate the first estimated position is less than a predetermined number of times during the period (detection period) from the time when the last calculation of the second estimated position to the time when the second calculation of the second estimated position is currently performed is less than a predetermined number of times, the second calculation unit 50 does not need to perform the optimization calculation using the first estimated position as the initial solution. In this case, the second calculation unit 50 performs the optimization calculation using the third estimated position as the initial solution to calculate the second estimated position. On the other hand, if the number of times that the second calculation unit 50 has been able to calculate the first estimated position during the detection period is equal to or greater than a predetermined number of times, the second calculation unit 50 may perform the optimization calculation using the first estimated position as the initial solution to calculate the second estimated position. In this case, the second calculation unit 50 may use the last calculated first estimated position among the multiple first estimated positions as the initial solution. Here, the predetermined number of times may be set arbitrarily.

[0064] Furthermore, if the second calculation unit 50 is able to calculate the first estimated position consecutively a predetermined number of times or more during the detection period, the second calculation unit 50 may calculate the second estimated position by performing an optimization calculation using the first estimated position as an initial solution. In this case, the second calculation unit 50 may use the last calculated first estimated position among the multiple first estimated positions as the initial solution. On the other hand, if the second calculation unit 50 is unable to calculate the first estimated position consecutively a predetermined number of times or more during the detection period (i.e., if the number of times the first estimated position was able to be calculated consecutively is less than the predetermined number), the second calculation unit 50 may not use the first estimated position as the initial solution, but may perform an optimization calculation using the third estimated position as the initial solution to calculate the second estimated position.

[0065] In this way, when the marker 100 cannot be identified or the first estimated position cannot be calculated accurately, the third estimated position can be used instead of the first estimated position, thereby preventing a decrease in the accuracy of the estimation of the vehicle's own position.

[0066] (Processing Flow) Next, a processing flow for setting the self-position by the control device 28 will be described. Fig. 9 is a flowchart for explaining the processing flow for setting the self-position.

[0067] 9 , the control device 28 causes the detection control unit 42 to acquire the detection results of the surrounding environment by the sensor 26A (step S10), the extraction unit 44 to extract the sign detection results, and the sign acquisition unit 46 to attempt to identify the detected sign 100 based on the sign detection results (step S12). If the sign 100 is identified (step S14; Yes), the control device 28 causes the sign acquisition unit 46 to acquire position information of the identified sign 100, and the first calculation unit 48 to calculate a first estimated position of the moving object 10 based on the position information of the sign 100 and the sign detection results (step S16). Then, the control device 28 causes the second calculation unit 50 to calculate a second estimated position of the moving object 10 by optimization calculation using the first estimated position as an initial solution (step S18). On the other hand, if the control device 28 cannot identify the marker 100 (step S14; No), the second calculation unit 50 calculates a third estimated position of the mobile object 10 from the past self-position and the detection result of the internal sensor 26B (step S22), and calculates a second estimated position of the mobile object 10 by optimization calculation using the first estimated position as the initial solution (step S24). The control device 28 sets the second estimated position as the self-position by the movement control unit 52 (step S20), and if the processing is to be ended (step S26; Yes), the processing is ended, but if not to be ended (step S26; No), the control device 28 returns to step S10 and estimates the self-position at the next timing.

[0068] (Effect) As described above, in this embodiment, the first estimated position of the mobile object 10 calculated from the detection results of the marker 100 is used as an initial solution, and the self-position (second estimated position) of the mobile object 10 is calculated by an optimization calculation of the map information and the detection results. By estimating the self-position in this manner, deviation of the estimated self-position from the actual self-position can be suppressed, and the self-position can be appropriately estimated. As a result of extensive research, the inventors have found that the first estimated position calculated from the detection results of the marker 100 is unlikely to deviate significantly, but that accuracy in finer details, such as on the order of centimeters, may be low. Furthermore, the inventors have found that optimization calculations using map information and detection results can achieve high accuracy in finer details, such as on the order of centimeters, if the calculations are successful, but can result in large deviations if the calculations are not successful. In this embodiment, this characteristic is utilized to perform optimization calculations using the map information and the detection results while using the first estimated position as an initial solution, thereby suppressing large deviations and increasing accuracy in finer details. In other words, by starting the optimization calculation from the first estimated position that does not deviate significantly from the correct answer, it is possible to suppress large deviations in the optimization calculation, and while suppressing large deviations, accuracy can be improved from a fine point of view.

[0069] Second Embodiment Next, a second embodiment will be described. The second embodiment differs from the first embodiment in that, if an optimization calculation using the second estimated position is abnormal, the third estimated position is set as the self-position. Descriptions of parts of the second embodiment that are common to the first embodiment will be omitted.

[0070] The second calculation unit 50 of the second embodiment attempts to calculate a second estimated position by optimization calculation in the same manner as in the first embodiment. The second calculation unit 50 of the second embodiment determines whether the optimization calculation is abnormal, and if not, sets the second estimated position calculated by the optimization calculation as the self-position to be used for movement control. On the other hand, if the optimization calculation is abnormal, the second calculation unit 50 does not set the second estimated position calculated by the optimization calculation as the self-position, and sets a third estimated position (an estimated position calculated based on a previously calculated self-position and the detection result of the internal sensor 26B) as the self-position.

[0071] The criteria for determining whether the optimization calculation is abnormal may be arbitrary. For example, if the difference between the second estimated position calculated by the optimization calculation and the third estimated position calculated based on the past self-position and the detection results of the internal sensor 26B is equal to or greater than a predetermined threshold, the second calculation unit 50 determines that the optimization calculation is abnormal and sets the third estimated position as the self-position. Also, for example, if the difference between the second estimated position calculated by the optimization calculation and the first estimated position (initial solution) estimated from the position information of the marker 100 is equal to or greater than a predetermined threshold, the second calculation unit 50 determines that the optimization calculation is abnormal and sets the third estimated position as the self-position. Also, for example, if the number of iterations (number of repeated calculations) of the optimization calculation is equal to or greater than a predetermined threshold, the second calculation unit 50 determines that the optimization calculation is abnormal and sets the third estimated position as the self-position. Also, for example, if the convergence residual of the convergence solution of the optimization calculation is equal to or greater than a predetermined threshold, the second calculation unit 50 determines that the optimization calculation is abnormal and sets the third estimated position as the self-position.

[0072] In this way, when the optimization calculation is abnormal, that is, for example, when it is determined that the accuracy of the optimization calculation is low, the self-position estimation accuracy can be suppressed from decreasing by estimating the self-position without using the results of the optimization calculation.

[0073] In the above description, the third estimated position is set as the vehicle's own position when the optimization calculation is abnormal, but this is not limited to this. For example, if the optimization calculation is abnormal but the first estimated position has been calculated, the second calculation unit 50 may set the first estimated position as the vehicle's own position. That is, in this case, if the optimization calculation is abnormal and the first estimated position has not been calculated, the second calculation unit 50 sets the third estimated position as the vehicle's own position.

[0074] Third Embodiment Next, a third embodiment will be described. The third embodiment differs from the second embodiment in that the detection result of the sensor 26A is added as map information data. Descriptions of parts of the third embodiment that are common to the second embodiment will be omitted.

[0075] In this embodiment, the second calculation unit 50 attempts to calculate a second estimated position by optimization calculation using the same method as in the first embodiment. If the optimization calculation is not abnormal, the map acquisition unit 40 of this embodiment adds the point cloud resulting from the detection by the sensor 26A (the point cloud used to calculate the second estimated position) to the point cloud of the map information, thereby updating the map information. On the other hand, if the optimization calculation is abnormal, the map acquisition unit 40 does not add the point cloud resulting from the detection by the sensor 26A (the point cloud used to calculate the second estimated position) to the point cloud of the map information.

[0076] 10 is a schematic diagram illustrating an example of a process for adding detection results to map information. Specifically, in this embodiment, as shown in steps S100 and S102 of FIG. 10 , the map acquisition unit 40 converts the point group A of the detection results based on the coordinate system of the moving body 10 (the positions of the measurement points in the coordinate system of the moving body 10) into the point group A based on the coordinate system of the target area AR. In this case, the map acquisition unit 40 uses the currently set self-position of the moving body 10 (the position and orientation of the moving body 10 in the coordinate system of the target area AR) to convert the point group A of the detection results based on the coordinate system of the moving body 10 into the point group A based on the coordinate system of the target area AR. Then, as shown in step S104 of FIG. 10 , the map acquisition unit 40 adds the point group A based on the coordinate system of the target area AR to the point group Aa included in the map information, thereby updating the map information. That is, the updated map information includes the point group Aa contained in the original map information and the point group A of the detection result.

[0077] In this way, by adding the point cloud A of the detection results to the point cloud Aa of the map information, more precise map information can be obtained, and the calculation accuracy of the second estimated position can be improved. Furthermore, for example, there is a case where an object (e.g., a truck, a package, etc.) other than the installed object W that was not initially placed is placed in the facility F. In that case, by adding the point cloud A of the detection results to the point cloud Aa of the map information, the point cloud of the object other than the installed object W that was not initially placed can also be added to the map information, and the map information can be updated to suit the situation of the facility F, thereby improving the calculation accuracy of the second estimated position.

[0078] In the third embodiment, the criteria for determining whether the optimization calculation is abnormal may be the same as those in the second embodiment. However, in the third embodiment, in addition to the criteria in the second embodiment, the following criteria may be added. For example, the map acquisition unit 40 may determine that the optimization calculation is abnormal if the first estimated position cannot be calculated (i.e., if the marker 100 cannot be identified). In other words, in this case, the optimization calculation is performed using the third estimated position that does not use the marker 100 as the initial solution. Even if this optimization calculation is successful, it is preferable not to add the point cloud of the detection results of the sensor 26A to the map information because the detection results of the marker 100 were not used as the initial solution. On the other hand, if the first estimated position can be calculated and the optimization calculation is successful, the point cloud of the detection results of the sensor 26A is added to the map information. Furthermore, for example, the map acquisition unit 40 may determine that the optimization calculation is abnormal if the distance between the self-position of the mobile object 10 when the map information was last updated and the self-position of the mobile object 10 set this time is less than a threshold. In other words, in this case, even if the optimization calculation is successful, by not updating the map if the distance since the previous map update is not large, excessive map updates in nearby locations can be prevented. On the other hand, if the distance between the self-position of the mobile unit 10 when the map information was last updated and the self-position of the mobile unit 10 set this time is equal to or greater than a threshold and the optimization calculation is successful, the point cloud detected by the sensor 26A is added to the map information. This allows point clouds to be added at positions that are appropriately spaced apart, making the map information more accurate.

[0079] Here, the point cloud A detected by a single scan of the sensor 26A is defined as a point cloud set. In this case, the map acquisition unit 40 preferably maintains the number of point cloud sets added to the map information at or below a predetermined number. That is, when the optimization calculation is normal, the map acquisition unit 40 counts the number of point cloud sets already added to the map information. Then, when the number of point cloud sets already added to the map information is less than the predetermined number, the map acquisition unit 40 adds the point cloud set resulting from the current detection to the map information. On the other hand, when the number of point cloud sets already added to the map information has reached the predetermined number, the map acquisition unit 40 removes the oldest point cloud set included in the map information from the map information and then adds the point cloud set resulting from the current detection. This prevents the map information from including more than the predetermined number of point cloud sets while adding the latest point cloud set. This prevents the data volume of the map information from becoming too large and increases the accuracy of the map information.

[0080] The map acquisition unit 40 may select whether to use updated map information (map information to which the point cloud A of the detection results has been added) or non-updated map information (map information to which the point cloud A of the detection results has not been added) as the map information for calculating the second estimated position. For example, the map acquisition unit 40 may acquire information on a time period in which the updated map information should be used from an external device (e.g., a management device that manages the facility F), and use the updated map information during that time period and non-updated map information during time periods other than that time period.

[0081] (Effect) As described above, the control device 28 according to the first aspect of the present disclosure includes a map acquisition unit 40 that acquires map information of a target area AR in which the mobile object 10 moves, a detection control unit 42 that acquires detection results of the surrounding environment by the sensor 26A mounted on the mobile object 10, a first calculation unit 48 that calculates a first estimated position of the mobile object 10 in the target area AR based on a sign detection result that detects a sign 100 among the detection results and position information of the sign 100 in the target area AR, and a second calculation unit 50 that performs an optimization calculation based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the mobile object 10 in the target area AE. According to the present disclosure, the first estimated position of the mobile object 10 calculated from the detection results of the sign 100 is used as an initial solution to calculate the self-position (second estimated position) of the mobile object 10 through an optimization calculation of the map information and the detection results. By estimating the self-position in this manner, deviation of the estimated self-position from the actual self-position can be suppressed, thereby enabling the self-position to be appropriately estimated.

[0082] A control device 28 according to a second aspect of the present disclosure is the control device 28 according to the first aspect, in which the map acquisition unit 40 acquires data including a point cloud indicating the environment of the target area AR as map information, and the detection control unit 42 acquires the data including the point cloud as a detection result. According to the present disclosure, the self-location can be appropriately estimated by estimating the self-location using the point cloud.

[0083] The control device 28 according to a third aspect of the present disclosure is the control device 28 according to the first or second aspect, and further includes an extraction unit 44 that extracts, from the detection results, a sign detection result that detects a sign 100, and a sign acquisition unit 46 that identifies the sign 100 detected by the sensor 26A based on a feature amount of the sign detection result and acquires position information of the identified sign 100. According to the present disclosure, the position of the sign 100 is identified without using map information and a first estimated position is calculated, thereby making it possible to appropriately estimate the self-location.

[0084] A control device 28 according to a fourth aspect of the present disclosure is the control device 28 according to any one of the first to third aspects, in which a first calculation unit 48 calculates the relative position of the moving object 10 with respect to the sign 100 based on the sign detection result, and calculates a first estimated position from the relative position of the moving object 10 and position information of the sign 100. According to the present disclosure, the self-position can be appropriately estimated.

[0085] A control device 28 according to a fifth aspect of the present disclosure is the control device 28 according to any one of the first to fourth aspects, in which the second calculation unit 50 calculates, as the second estimated position, the position of the moving object 10 where the deviation between the map information and the detection result is optimized. According to the present disclosure, the self-position can be appropriately estimated.

[0086] A control device 28 according to a sixth aspect of the present disclosure is the control device 28 according to any one of the first to fifth aspects, wherein, when the second calculation unit 50 is unable to calculate the first estimated position, it calculates a third estimated position of the moving body 10 based on the past self-position (second estimated position) of the moving body 10 and the detection result of the internal sensor 26B that detects an output parameter indicating the control output of the moving body 10. Then, the second calculation unit 50 performs an optimization calculation using the third estimated position as an initial solution to calculate the second estimated position. According to the present disclosure, it is possible to suppress a decrease in the estimation accuracy of the self-position.

[0087] A control device 28 according to a seventh aspect of the present disclosure is the control device 28 according to any one of the first to sixth aspects, wherein the second calculation unit 50 calculates the second estimated position using the first estimated position as an initial solution if the number of times the first estimated position was calculated is a predetermined number or more during the period (detection period) from the time the self-position (second estimated position) was last calculated to the time the self-position (second estimated position) is calculated this time, and calculates the second estimated position using the third estimated position as the initial solution if the number of times the first estimated position was calculated is less than the predetermined number. According to the present disclosure, it is possible to suppress a decrease in the estimation accuracy of the self-position.

[0088] A control device 28 according to an eighth aspect of the present disclosure is the control device 28 according to any one of the first to seventh aspects, wherein, when the optimization calculation is abnormal, the second calculation unit 50 calculates the second estimated position based on the past self-position (second estimated position) of the mobile object 10 and the detection result of an internal sensor that detects an output parameter indicating the control output of the mobile object. According to the present disclosure, it is possible to suppress a decrease in the estimation accuracy of the self-position.

[0089] A control device 28 according to a ninth aspect of the present disclosure is the control device 28 according to the second aspect, and when the optimization calculation is not abnormal, the map acquisition unit 40 converts the point cloud of the detection result based on the coordinate system of the moving body 10 into a point cloud based on the coordinate system of the target area AR, and adds the coordinate-converted point cloud to the map information. According to the present disclosure, the map information can be made more accurate, thereby enabling more appropriate estimation of the self-position.

[0090] A moving body 10 according to a tenth aspect of the present disclosure includes the control device 28 according to any one of the first to ninth aspects. According to the present disclosure, the moving body 10 can appropriately estimate its own position.

[0091] A sign 100 according to an eleventh aspect of the present disclosure is a sign used for self-location estimation of a moving body 10, and has a plurality of reflective members 110, at least some of which have different reflectances, and at least one of the number and arrangement order of the reflective members 110 is set to a pattern that allows the moving body 10 to distinguish from other signs 100. According to the present disclosure, the moving body 10 can identify which sign 100 it is, and therefore can appropriately estimate its self-location.

[0092] A position estimation method according to a twelfth aspect of the present disclosure includes the steps of acquiring map information of a target area AR in which a mobile object 10 moves, acquiring detection results of the surrounding environment by a sensor 26A mounted on the mobile object 10, calculating a first estimated position of the mobile object 10 in the target area AR based on a sign detection result that detects a sign 100 among the detection results and position information of the sign 100 in the target area AR, and performing an optimization calculation based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the mobile object 10 in the target area AE. According to the present disclosure, it is possible to appropriately estimate the self-position.

[0093] A program according to a thirteenth aspect of the present disclosure causes a computer to execute the following steps: acquiring map information of a target area AR in which a mobile object 10 moves; acquiring detection results of the surrounding environment by a sensor 26A mounted on the mobile object 10; calculating a first estimated position of the mobile object 10 in the target area AR based on a sign detection result that detects a sign 100 among the detection results and position information of the sign 100 in the target area AR; and performing an optimization calculation based on the map information and the detection results using the first estimated position as an initial solution to calculate a second estimated position of the mobile object 10 in the target area AE. According to the present disclosure, it is possible to appropriately estimate the self-location.

[0094] Although the embodiments of the present disclosure have been described above, the embodiments are not limited to the contents of these embodiments. Furthermore, the above-described components include those that can be easily imagined by a person skilled in the art, those that are substantially the same, and those that are within the so-called equivalent range. Furthermore, the above-described 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-described embodiments.

[0095] REFERENCE SIGNS LIST 10 Mobile object 26A Sensor 40 Map acquisition unit 42 Detection control unit 44 Extraction unit 46 Sign acquisition unit 48 First calculation unit 50 Second calculation unit 52 Movement control unit 100 Sign 110 Reflection member AR Target area W Installed object

Claims

1. A control device comprising: a map acquisition unit that acquires map information of a target area in which a moving body moves; a detection control unit that acquires detection results of the surrounding environment by a sensor mounted on the moving body; a first calculation unit that calculates a first estimated position of the moving body in the target area based on a sign detection result that detects a sign from the detection results and position information of the sign in the target area; and a second calculation unit that performs optimization calculations based on the map information and the detection results, using the first estimated position as an initial solution, to calculate a second estimated position of the moving body in the target area.

2. The control device according to claim 1, wherein the map acquisition unit acquires data including a point cloud indicating the environment of the target area as the map information, and the detection control unit acquires data including the point cloud as the detection result.

3. The control device according to claim 1 or claim 2, further comprising: an extraction unit that extracts the sign detection result, which is a detection result of detecting the sign, from the detection result; and a sign acquisition unit that identifies the sign detected by the sensor based on features of the sign detection result and acquires position information of the identified sign.

4. A control device as described in claim 1 or claim 2, wherein the first calculation unit calculates the relative position of the moving body with respect to the sign based on the sign detection result, and calculates the first estimated position from the relative position of the moving body and position information of the sign.

5. A control device according to claim 1 or claim 2, wherein the second calculation unit calculates the position of the moving body where the deviation between the map information and the detection result is optimized as the second estimated position.

6. A control device as described in claim 1 or claim 2, wherein, when the first estimated position cannot be calculated, the second calculation unit calculates a third estimated position of the moving body based on the past second estimated position of the moving body and the detection results of an internal sensor that detects an output parameter indicating the control output of the moving body, and performs an optimization calculation using the third estimated position as an initial solution to calculate the second estimated position.

7. The control device described in claim 6, wherein the second calculation unit calculates the second estimated position using the first estimated position as an initial solution if the number of times the first estimated position was able to be calculated is a predetermined number or more during the period from the last calculation of the second estimated position to the current calculation of the second estimated position, and calculates the second estimated position using the third estimated position as an initial solution if the number of times the first estimated position was able to be calculated is less than the predetermined number of times.

8. A control device as described in claim 1 or claim 2, wherein, when the optimization calculation is abnormal, the second calculation unit calculates the second estimated position based on the past second estimated position of the moving body and the detection results of an internal sensor that detects an output parameter indicating the control output of the moving body.

9. The control device according to claim 2, wherein, when the optimization calculation is not abnormal, the map acquisition unit converts the point cloud of the detection result based on the coordinate system of the moving body into a point cloud based on the coordinate system of the target area, and adds the converted point cloud to the map information.

10. A mobile object comprising the control device according to claim 1 or 2.

11. A sign used for estimating the self-position of a moving body, comprising a plurality of reflective elements, at least some of which have different reflectivities, and at least one of the number and arrangement order of the reflective elements is set to a pattern that allows it to be distinguished from other signs.

12. A position estimation method comprising: a step of acquiring map information of a target area in which a mobile body is moving; a step of acquiring detection results of the surrounding environment by a sensor mounted on the mobile body; a step of calculating a first estimated position of the mobile body in the target area based on a sign detection result that detects a sign from the detection results and position information of the sign in the target area; and a step of performing an optimization calculation based on the map information and the detection results, using the first estimated position as an initial solution, to calculate a second estimated position of the mobile body in the target area.

13. A program that causes a computer to execute the following steps: acquiring map information of a target area in which a mobile body is moving; acquiring detection results of the surrounding environment by a sensor mounted on the mobile body; calculating a first estimated position of the mobile body in the target area based on sign detection results that detect signs from the detection results and position information of the signs in the target area; and performing optimization calculations based on the map information and the detection results, using the first estimated position as an initial solution, to calculate a second estimated position of the mobile body in the target area.

Images