Control device, moving body, sign, position estimation method, and program

Abstract

To appropriately estimate the position of a moving body.SOLUTION: A control device 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 mobile body; a first calculation unit that calculates a first estimated position of the mobile body in the target area based on a sign detection result that detects signs from the detection results and position information of the signs 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 mobile body in the target area.SELECTED DRAWING: Figure 1

Description

[Technical Field]

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

[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 the reflected wave 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 the output pattern of the reflected wave 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 position of the identified sign on the environmental map. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 5983088 Summary of the Invention [Problem to be solved by the invention]

[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. [Means for solving the problem]

[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 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 moving body in the target area. [Effects of the Invention]

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

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

[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 object according to a first embodiment. In this embodiment, the mobile object 10 belongs to a facility F and moves within the facility F. The facility F is, for example, a facility managed by logistics, such as a warehouse, but may be any facility that operates the mobile object 10, and may be indoors or outdoors. The mobile object 10 picks up and transports an object placed within a target area AR of the facility F. The target area AR is an area where objects are placed and where the mobile object 10 moves, such as the floor of the facility F. In this embodiment, the object transported by the mobile object 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 object 10 is not limited to a device that transports objects, but may be a device that moves within the facility F for any purpose.

[0015] (region) 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, a direction perpendicular to the X and Y directions, more specifically, a 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 viewed from the Z direction, with the X direction being 0°.

[0017] (sign) Signs 100 are placed in the target area AR (facility F). The signs 100 refer to objects that serve as landmarks for estimating the self-position of the moving body 10. The signs 100 are objects that can be detected by a sensor 26A of the moving 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 an installation W1 that is a pillar and one on an installation W2 that is a wall, but the installation positions and number of signs 100 are 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 member that can be detected 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 able to be distinguished from other signs) based on the detection result by 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 reflectances. 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 reflectance than the reflective member 110A. In this embodiment, the reflective member 110B is a plate-like member and has a rectangular shape 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 reflectance than the reflective member 110B. In this embodiment, the reflective member 110A has a rectangular shape when viewed from the 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-like 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 reflections 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 reflecting members 110 (in this example, reflecting members 110A and 110B) are aligned in a direction perpendicular to the Z direction (horizontal direction). In the example of Fig. 2, the reflecting 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 reflecting 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. That is, 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 moving body 10 detects each sign 100, the feature amounts of the detection results differ from one another. As a result, the moving 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 (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, and 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. FIG. 2 illustrates an example in which the number of reflective members 110 between the two reflective members 110A at both ends is seven. In this embodiment, the arrangement order of the reflective members 110A and 110B between the two reflective members 110A at both ends differs 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, 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 each of 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) FIG. 4 is a schematic diagram of the configuration of a moving body. The moving body 10 is a device that can move automatically. In this embodiment, the moving body 10 is a non-holonomic system that cannot move sideways, but is not limited to this. In this embodiment, the moving body 10 is a device that can transport a target object. More specifically, in this embodiment, the moving body 10 is a forklift, and more specifically, a so-called AGV (Automated Guided Vehicle) or AGF (Automated Guided Forklift). However, the moving body 10 is not limited to a forklift that transports a target object, and may be any device that can move automatically.

[0026] As shown in FIG. 2, the mobile body 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 direction and the fore-and-aft 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, in the fore-and-aft 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 moving body 10 and the posture of the object relative to the moving 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, 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 a sensor mounted on the moving body 10 and detects output parameters of the moving body 10. The output parameters are parameters that indicate the control output of the moving body 10. More specifically, the output parameters indicate the degree to which the moving body 10 operates under control, and can be said to be output values ​​from the moving body 10 when a control input is given to the moving body 10. That is, the control device 28 gives control inputs to the drive mechanism (such as an actuator) of the moving body 10 so that the moving body 10 moves in a desired direction and at a desired speed. The drive mechanism of the moving body 10 operates in response to the control inputs, and output parameters corresponding to the control inputs are detected by the internal sensor 26B. The output parameters may be any parameters that indicate the control output of the moving body, but in this embodiment, they are the rotation speed of the wheels 20A of the moving body 10 and the steering angle of the moving 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 moving body 10 (the amount of change in the direction of the wheels 20A). A sensor for detecting the rotation speed and a sensor for detecting the steering angle may be provided as the internal sensor 26, or a single sensor may be used to detect 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 for 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 memory 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 memory 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 memory device such as a ROM, and an external memory 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] (Controller processing) Next, the processing contents of the control device 28 will be described.

[0034] (Movement control of moving objects) The movement control unit 52 of the control device 28 controls the movement of the moving object 10 by controlling the movement mechanisms such as the drive unit and steering of the moving object 10. The movement control unit 52 moves the moving object 10 so as to follow a set route R (e.g., FIG. 1) of the moving object 10. In this case, the control device 28 successively estimates the position and attitude (self-position) of the moving object 10, and moves the moving object 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 moving object 10 (second estimated position described later) by performing optimization calculations 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 object 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 method for estimating the self-location will be specifically described below.

[0035] (Getting 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 installations 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 installations W in the coordinate system of the target area AR. In this embodiment, the map information of the target area AR is data in which the positions (coordinates) of the installations W are expressed 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 by 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] (Surrounding environment detection) 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, in a configuration in which the sensor 26A emits laser light, the detection control unit 42 causes the sensor 26A to emit the 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 point where the reflected light is reflected in the coordinate system of the moving 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 point where the reflected light is reflected in the coordinate system of the moving 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 point where the reflected light is reflected. Note that the point cloud includes information on the position of the point where the reflected light is reflected (in this example, the position in the coordinate system of the moving 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 object 10. The detection control unit 42 acquires the detection results of the output parameters of the moving object 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 marker detection results) The extraction unit 44 extracts a sign detection result, which is a detection result 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 a detection result having a predetermined feature amount from the detection results of the surrounding environment as the sign detection result. If the detection results of the surrounding environment do not include a detection result having the predetermined feature amount, 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 amount 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 each point cloud 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 and obtained by a single scan by the sensor 26A. The threshold value 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. 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 sign 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 horizontally arranged point groups A as a sign detection result (a point group where the sign 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 sign detection result (a point group where the sign 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 sign 100 is not identified, and no sign detection result is extracted. Also, for example, if point group A1 is located at a position farther away from point group A1a than the predetermined distance D on the other end side (the right side in the example of FIG. 6), that point group A1 is set as point group A1a on the horizontal end side, and it is determined whether there is another sign detection result.

[0042] For example, if multiple sets of point clouds A (scan lines) from point cloud A1a to point cloud A1b are obtained due to multiple scans performed by sensor 26A, 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 clouds A, any scan lines whose distance from sensor 26A to point cloud A is outside a predetermined range (i.e., points that are too close or too far from sensor 26A), or any scan lines whose length from point cloud A1a to point cloud A1b is outside a predetermined range (i.e., points that are too long or too short). Furthermore, Euclidean clustering may be performed at the position of the starting point (point cloud 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 clouds 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, it is possible to reduce the number of point clouds to be searched for sign detection results, 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 result (detection result of the output parameter) 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 result 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 signs) 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, from the feature amount of the sign detection result, which of the signs 100 in the target area AR is the sign 100 currently detected by the sensor 26A.

[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 in this embodiment will be described in more detail below. FIG. 7 is a schematic diagram showing 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 sign 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 sign 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] Then, as in step S3 of Fig. 7, the marker acquisition unit 46 normalizes the evaluation index E of 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 of each section C. In this case, a correspondence between the sign 100 and the correct value of the evaluation index Ea of each section is set in advance for each sign 100. The sign acquisition unit 46 acquires the correspondence, extracts the correct value of the evaluation index Ea that matches the calculated evaluation index Ea of each section C in the correspondence, and identifies the sign 100 associated with the 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 of 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 learn 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, and the resulting sign 100 (the label with the highest matching probability output from the AI ​​model) may be identified as the detected sign 100. 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] (Obtaining location information of signs) 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 the correspondence relationship and acquires the position information associated with the identified sign 100 in the 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 object 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 attitude of the moving object 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 sign 100 (the position of the moving object 10 in the coordinate system of 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 the position information of the sign 100. That is, since the sign detection result indicates the position of the sign 100 with respect to the moving object 10, the position of the moving object 10 in the coordinate system of the sign 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 sign detection result as the origin position of the coordinate system of the sign 100. Then, the unit vector in the Z direction at the origin position is set to the direction of the Z axis, the cross product of the Z axis and the scan line is set to the direction of the Y axis as a normal, and the cross product of the Z axis and the Y axis is set to the direction of the X axis. Then, the first calculation unit 48 calculates the position and orientation of the moving object 10 (sensor 26A) in this coordinate system of the X-axis, Y-axis, and Z-axis as the position of the moving object 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 moving object 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 moving object 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 sign detection results used to detect the sign 100 and detection results detecting objects other than the sign 100 (for example, 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 to this, and may include at least detection results detecting objects other than the sign 100.

[0058] The second calculation unit 50 performs an optimization calculation to obtain, as an optimal solution, the position of the moving body 10 at 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), while using the first estimated position as an initial solution, 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 attitude of the moving body 10 are at the first estimated position, and then performs an optimization calculation to repeatedly update the position and attitude of the moving body 10 so as to reduce the deviation amount, and calculates the position and attitude 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 attitude of the moving object 10 that optimizes the amount of deviation between the point cloud A included in the detection results and a part of the point cloud Aa included in the map information. Note that the optimization calculation may use, for example, a point cloud alignment 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 feature amounts of the image in the map information and the feature amounts of the image in the detection results.

[0060] (Determining 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 self-position that has been set. 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 performing optimization calculation without using the first estimated position) However, if the first estimated position is not 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 the optimization calculation using the first estimated position as the initial solution. In this case, the second calculation unit 50 performs the optimization calculation to calculate the second estimated position using the third estimated position obtained using a method other than the method using the position of the sign 100 as the initial solution. In this case, the optimization calculation method is the same as the above-mentioned method 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 an estimated attitude of the moving object 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 object 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 the 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 first estimated position has been calculated is less than a predetermined number of times during the period (detection period) from the time when the last second estimated position was calculated until the time when the second estimated position is calculated this time, the second calculation unit 50 may not 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 first estimated position has been calculated 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. The predetermined number of times here 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 is used instead of the first estimated position, thereby preventing a decrease in the estimation accuracy of the self-position.

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

[0067] 9, the control device 28 acquires the detection result of the surrounding environment by the sensor 26A using the detection control unit 42 (step S10), extracts the sign detection result using the extraction unit 44, and attempts to identify the detected sign 100 using the sign acquisition unit 46 based on the sign detection result (step S12). If the sign 100 is identified (step S14; Yes), the control device 28 acquires position information of the identified sign 100 using the sign acquisition unit 46, and calculates a first estimated position of the moving object 10 based on the position information of the sign 100 and the sign detection result using the first calculation unit 48 (step S16). Then, the control device 28 calculates a second estimated position of the moving object 10 using the second calculation unit 50 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 moving 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 moving object 10 by optimization calculation using the first estimated position as an 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), ends this processing, or if not to be ended (step S26; No), 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 moving object 10 calculated from the detection result of the sign 100 is used as an initial solution, and the self-position (second estimated position) of the moving object 10 is calculated by optimization calculation of the map information and the detection result. 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 result of the sign 100 is unlikely to deviate significantly, but that precision 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 precision 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 detection results while using the first estimated position as an initial solution, thereby suppressing large deviations and improving precision 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, it is possible to improve accuracy 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. Explanations of the configurations of the second embodiment 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 way as in the first embodiment. The second calculation unit 50 of the second embodiment determines whether the optimization calculation is abnormal, and if it is not abnormal, 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 result 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. 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. 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. 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 own position when the optimization calculation is abnormal, but this is not limited to this. For example, when the optimization calculation is abnormal and the first estimated position can be calculated, the second calculation unit 50 may set the first estimated position as the own position. That is, in this case, when the optimization calculation is abnormal and the first estimated position cannot be calculated, the second calculation unit 50 sets the third estimated position as the 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. Explanations 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. Then, if the optimization calculation is not abnormal, the map acquisition unit 40 of this embodiment adds the point cloud of the detection results of the sensor 26A (the point cloud used to calculate the second estimated position) to the point cloud of the map information and updates the map information. On the other hand, if the optimization calculation is abnormal, the map acquisition unit 40 does not add the point cloud of the detection results of the sensor 26A (the point cloud used to calculate the second estimated position) to the point cloud of the map information.

[0076] FIG. 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 performs coordinate conversion on the point group A of the detection results based on the coordinate system of the moving body 10 (positions of 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 perform coordinate conversion on 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 to update the map information. That is, the updated map information includes the point group Aa included in the original map information and the point group A of the detection results.

[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 installation 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 installation 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 set. 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 sign 100 cannot be identified). That is, in this case, the optimization calculation is performed using the third estimated position that does not use the sign 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 sign 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 moving object 10 when the map information was last updated and the self-position of the moving object 10 set this time is less than a threshold. In other words, in this case, even if the optimization calculation is successful, the map is not updated if the distance since the previous map update is not large, thereby preventing excessive map updates in nearby locations. On the other hand, if the distance between the self-position of the moving body 10 when the map information was last updated and the self-position of the moving body 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 of the current detection result to the map information. On the other hand, when the number of point cloud sets already added to the map information reaches 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 of the current detection result. 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 improves 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 moving object 10 moves; a detection control unit 42 that acquires detection results of the surrounding environment by the sensor 26A mounted on the moving object 10; a first calculation unit 48 that calculates a first estimated position of the moving 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 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 object 10 in the target area AE. According to the present disclosure, the first estimated position of the moving 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 moving object 10 through optimization calculations of the map information and the detection results. Estimating the self-position in this manner reduces deviation of the estimated self-position from the actual self-position, allowing 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 the 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 the second calculation unit 50, when unable to calculate the first estimated position, calculates a third estimated position of the moving object 10 based on the past self-position (second estimated position) of the moving object 10 and the detection result of the internal sensor 26B that detects an output parameter indicating the control output of the moving object 10. Then, the second calculation unit 50 performs 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 a control device 28 according to any one of the first to sixth aspects, in which 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 has been calculated is a predetermined number or more during the period (detection period) from the last calculation of the self-position (second estimated position) to the current calculation of the self-position (second estimated position), and calculates the second estimated position using the third estimated position as the initial solution if the number of times the first estimated position has been 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 moving body 10 and the detection result of an internal sensor that detects an output parameter indicating the control output of the moving body. 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, in which, 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-location.

[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-localization 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-localization.

[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 moving object 10 moves, acquiring detection results of the surrounding environment by a sensor 26A mounted on the moving object 10, calculating a first estimated position of the moving 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 moving 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 moving object 10 moves; acquiring detection results of the surrounding environment by a sensor 26A mounted on the moving object 10; calculating a first estimated position of the moving 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 moving object 10 in the target area AE. According to the present disclosure, the self-location can be appropriately estimated.

[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. [Explanation of symbols]

[0095] 10 Mobile 26A Sensor 40 Map Acquisition Section 42 Detection control section 44 Extraction part 46 Tag Acquisition Department 48 First Calculation Section 50 Second calculation unit 52 Movement control unit 100 signs 110 Reflective member AR target area W Installation

Claims

1. a map acquisition unit that acquires map information of a target area in which the moving object moves; a detection control unit that acquires a result of detection 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 object 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; a second calculation unit that calculates a second estimated position of the moving object in the target area by performing an optimization calculation based on the map information and the detection result using the first estimated position as an initial solution; Including, Control device.

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

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

4. the first calculation unit calculates a relative position of the moving object with respect to the sign based on the sign detection result, and calculates the first estimated position from the relative position of the moving object and position information of the sign; The control device according to claim 1 or 2.

5. the second calculation unit calculates, as the second estimated position, a position of the moving object where a difference between the map information and the detection result is optimized; The control device according to claim 1 or 2.

6. The second calculation unit If the first estimated position cannot be calculated, a third estimated position of the moving body is calculated based on the past second estimated position of the moving body and a detection result of an internal sensor that detects an output parameter indicating a control output of the moving body; performing an optimization calculation using the third estimated position as an initial solution to calculate the second estimated position; The control device according to claim 1 or 2.

7. the second calculation unit calculates the second estimated position using the first estimated position as an initial solution when the number of times the first estimated position has been calculated is equal to or greater than a predetermined number of times during a period from when the second estimated position was calculated immediately before to when the second estimated position is calculated this time; and calculates the second estimated position using the third estimated position as an initial solution when the number of times the first estimated position has been calculated is less than the predetermined number of times. The control device according to claim 6.

8. 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 a detection result of an internal sensor that detects an output parameter indicating a control output of the moving body. The control device according to claim 1 or 2.

9. When the optimization calculation is not abnormal, the map acquisition unit performs coordinate transformation of the point cloud of the detection result based on the coordinate system of the moving body to a point cloud based on the coordinate system of the target area, and adds the coordinate-transformed point cloud to the map information. The control device according to claim 2 .

10. A control device according to claim 1 or 2, Mobile object.

11. A sign used for self-location estimation of a moving object, a plurality of reflecting members at least some of which have different reflectances; At least one of the number and arrangement order of the reflective members is set to a pattern that can be distinguished from other signs. sign.

12. acquiring map information of a target area in which the moving object moves; acquiring a result of detection of the surrounding environment by a sensor mounted on the moving body; calculating a first estimated position of the moving object in the target area based on a sign detection result in which a sign is detected from the detection results and position information of the sign in the target area; calculating a second estimated position of the moving object in the target area by performing an optimization calculation based on the map information and the detection result using the first estimated position as an initial solution; Including, Location estimation method.

13. acquiring map information of a target area in which the moving object moves; acquiring a result of detection of the surrounding environment by a sensor mounted on the moving body; calculating a first estimated position of the moving object in the target area based on a sign detection result in which a sign is detected from the detection results and position information of the sign in the target area; calculating a second estimated position of the moving object in the target area by performing an optimization calculation based on the map information and the detection result using the first estimated position as an initial solution; to the computer, program.

Images