Self-position estimation device

Abstract

To provide a self-location estimation device capable of improving an estimation accuracy of a self-location of a moving body.SOLUTION: A self-location estimation device 1 includes: a laser sensor 3 that irradiates laser light L to around a moving body 2, detects an object 6 existing around the moving body 2 by receiving reflected laser light L, and outputs point cloud data as detection data; a voxel division unit 12 that divides the point cloud data D of the laser sensor 3 into a plurality of voxels V to generate point cloud data Dv in units of voxels; a self-location estimation unit 13 that estimates the self-location of the moving body 2 based on the point cloud data Dv in units of voxels; and a voxel size determination unit 16 that determines a grid size of the voxel V according to environmental conditions around the moving body 2. Therein, which the voxel division unit 12 divides the point cloud data D of the laser sensor 3 into voxels V of the grid size determined by the voxel size determination unit 16.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a self-localization apparatus.

Background Art

[0002] As a conventional self-localization apparatus, for example, as described in Patent Document 1, a technique for estimating the self-position of a moving body by comparing reference point cloud (map point cloud) data stored in a storage unit with query point cloud (sensor point cloud) data acquired by LiDAR or the like, and performing voxel-based scan matching between the map point cloud and the sensor point cloud is known.

Prior Art Literature

Patent Literature

[0003]

Patent Document 1

Summary of the Invention

Problem to be Solved by the Invention

[0004] When dividing sensor point cloud data acquired by LiDAR or the like into a plurality of voxels and estimating the self-position of a moving body, if a voxel size (voxel grid size) suitable for the surrounding environment of the moving body is not set, this causes a decrease in the accuracy of self-localization.

[0005] An object of the present invention is to provide a self-localization apparatus capable of improving the estimation accuracy of the self-position of a moving body.

Means for Solving the Problem

[0006] One aspect of the present invention is a self-position estimation device for estimating the self-position of a moving object, comprising: a laser object detection unit that detects objects present around the moving object by irradiating a laser around the moving object and receiving the reflected light of the laser, and outputs point cloud data as detection data; a voxel division unit that divides the point cloud data from the laser object detection unit into a plurality of voxels to generate point cloud data in voxel units; a self-position estimation unit that estimates the self-position of the moving object based on the voxel-unit point cloud data generated by the voxel division unit; and a voxel size determination unit that determines the grid size of the voxels according to the environmental conditions around the moving object, wherein the voxel division unit divides the point cloud data from the laser object detection unit into voxels of the grid size determined by the voxel size determination unit.

[0007] In such a self-position estimation device, a laser object detection unit irradiates a laser around a moving object and receives the reflected light to detect objects present around the moving object, outputting point cloud data. The point cloud data from the laser object detection unit is then divided into multiple voxels to generate voxel-level point cloud data, and the self-position of the moving object is estimated based on this voxel-level point cloud data. At this time, the grid size of the voxels is determined according to the environmental conditions around the moving object, and the point cloud data from the laser object detection unit is divided into voxels of that grid size. Therefore, an appropriate voxel size is set according to the environmental conditions around the moving object. This improves the accuracy of the self-position estimation of the moving object.

[0008] The self-position estimation device further includes an indoor / outdoor determination unit that determines whether the environment in which the moving object is located is indoors or outdoors. The voxel size determination unit may increase the voxel grid size when the indoor / outdoor determination unit determines that the moving object is located outdoors, compared to when the indoor / outdoor determination unit determines that the moving object is located indoors.

[0009] Objects located indoors are often smaller in size than those located outdoors. Therefore, when a moving object is outdoors, the voxel grid size is increased compared to when the object is indoors, so that an appropriate voxel size is set regardless of whether the environment in which the moving object is located is indoors or outdoors.

[0010] The indoor / outdoor determination unit may determine whether the environment in which the moving object exists is indoors or outdoors based on the presence or absence of point clouds within a specific irradiation angle range in the point cloud data of the laser object detection unit.

[0011] In this configuration, the point cloud data from the laser object detection unit is used to determine whether the environment in which the moving object is located is indoors or outdoors. Therefore, it is not necessary to use dedicated sensors other than the laser object detection unit.

[0012] The laser object detection unit detects the distance to objects present around the moving object, and the voxel size determination unit may increase the voxel grid size as the distance from the laser object detection unit to the object increases.

[0013] When an object is close to the laser object detection unit, it is detected with fine granularity. However, as the object moves further away from the laser object detection unit, the detection granularity of the object becomes coarser. Therefore, by increasing the voxel grid size as the distance from the laser object detection unit to the object increases, an appropriate voxel size is set regardless of the distance from the laser object detection unit to the object.

[0014] The self-position estimation device further includes an average distance calculation unit that calculates the average distance from the laser object detection unit to the object, and the voxel size determination unit may increase the voxel grid size as the average value of the distance calculated by the average distance calculation unit increases.

[0015] In this configuration, the average distance from the laser object detection unit to the object is calculated, allowing for the setting of an even more appropriate voxel size. Effects of the Invention

[0016] According to the present invention, it is possible to improve the estimation accuracy of the self-position of a moving object. Brief Description of the Drawings

[0017] [Figure 1] FIG. 1 is a block diagram showing the configuration of a self-position estimation apparatus according to an embodiment of the present invention. [Figure 2] FIG. 2 is a diagram showing an example of point cloud data of a laser sensor and point cloud data in voxel units. [Figure 3] FIG. 3 is a schematic front view showing a state where a laser is emitted from the laser sensor to the surroundings of the moving object indoors and outdoors. [Figure 4] FIG. 4 is a schematic front view showing a state where a laser is emitted from the laser sensor to the surroundings of the moving object in an indoor environment where the distance from the laser sensor to an object is different. [Figure 5] FIG. 5 is a conceptual diagram showing an appropriate voxel grid size corresponding to the distance from the laser sensor to an object. [Figure 6] FIG. 6 is a flowchart showing the procedure of voxel size setting processing executed by the controller shown in FIG. 1. [Figure 7] FIG. 7 is a conceptual diagram showing the relationship among the voxel grid size, indoor determination values, outdoor determination values, and average distance values. Mode for Carrying Out the Invention

[0018] Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

[0019] FIG. 1 is a block diagram showing the configuration of a self-localization apparatus according to an embodiment of the present invention. In FIG. 1, the self-localization apparatus 1 of the present embodiment is mounted on a moving body 2 (see FIGS. 3 and 4) such as an industrial vehicle like a forklift or a towing tractor. The self-localization apparatus 1 is an apparatus that estimates the self-position of the moving body 2 during automatic driving of the moving body 2.

[0020] The self-localization apparatus 1 includes a laser sensor 3, a map data memory 4, and a controller 5.

[0021] The laser sensor 3 is a laser object detection unit that irradiates a laser L around the moving body 2 and receives reflected light of the laser L to detect an object 6 existing around the moving body 2 (see FIGS. 3 and 4), and outputs point cloud data as detection data. The laser sensor 3 is a distance measurement sensor that detects the distance to the object 6 existing around the moving body 2. The object 6 is a structure such as a building, a wall, or a pillar, for example.

[0022] As the laser sensor 3, for example, 3D LiDAR (Light Detection and Ranging), a laser range finder, or the like is used. The laser sensor 3 irradiates the laser L at a predetermined angle in a direction perpendicular to the horizontal direction (see FIGS. 3 and 4).

[0023] The map data memory 4 stores map data of an area where the moving body 2 travels. The map data is represented by a point cloud. The map data is generated in advance using the laser sensor 3.

[0024] The controller 5 is configured by a CPU, a RAM, a ROM, an input / output interface, and the like. The controller 5 includes a point cloud data acquisition unit 11, a voxel division unit 12, a self-localization estimation unit 13, an indoor / outdoor determination unit 14, an average distance calculation unit 15, and a voxel size determination unit 16.

[0025] The point cloud data acquisition unit 11 acquires point cloud data D from the laser sensor 3. A point cloud is a collection of reflection points of the laser L emitted from the laser sensor 3. Point cloud data D is three-dimensional data. The object 6 detected by the laser sensor 3 is represented as a point cloud, as shown in Figure 2(a). In Figure 2(a), a tree is shown as an example of an object 6 included in the point cloud data D of the laser sensor 3.

[0026] The voxel division unit 12 divides the point cloud data D of the laser sensor 3 acquired by the point cloud data acquisition unit 11 into multiple voxels V to generate point cloud data Dv on a voxel-by-voxel basis. Therefore, the objects 6 included in the point cloud data D are extracted on a voxel-by-voxel basis. As shown in Figure 2(b), the voxels V are a three-dimensional pixel space and are arranged in a grid. The voxels V have a cubic structure. Each voxel V has attribute data related to the object 6 included in the point cloud data Dv.

[0027] The point cloud data D shown in Figure 2(a) is divided into multiple voxels V to generate voxel-level point cloud data Dv as shown in Figure 2(b). At this time, each voxel V in the point cloud data Dv has attribute data such as the center coordinates of the voxel V, the number of detected points, the height from the ground, and the proportion of leaves, branches, and trunks in the tree.

[0028] The voxel division unit 12 divides the point cloud data D from the laser sensor 3 into voxels V with a grid size determined by the voxel size determination unit 16. The voxel size determination unit 16 will be described in detail later.

[0029] The self-position estimation unit 13 estimates the self-position of the moving object 2 based on the voxel-level point cloud data Dv generated by the voxel division unit 12. The self-position estimation unit 13 uses the SLAM (simultaneous localization and mapping) method to match the point cloud data D from the laser sensor 3 with the map data stored in the map data memory 4 to estimate the self-position of the moving object 2. SLAM is a self-position estimation technique that uses sensor data and map data to estimate the self-position.

[0030] The indoor / outdoor determination unit 14 determines whether the environment in which the moving object 2 is located is indoors or outdoors based on the point cloud data D of the laser sensor 3 acquired by the point cloud data acquisition unit 11. The indoor / outdoor determination unit 14 determines whether the environment in which the moving object 2 is located is indoors or outdoors based on the presence or absence of a point cloud in a specific irradiation angle range θ in the point cloud data D of the laser sensor 3.

[0031] When the moving object 2 is indoors, the laser L emitted from the laser sensor 3 towards the upward and sideways of the moving object 2 is reflected by an object 6 such as a wall, and the reflected light of the laser L is received by the laser sensor 3. For this reason, as shown in Figure 3, for example, whether the moving object 2 is indoors or outdoors is determined by the presence or absence of point clouds in the point cloud data D of the laser sensor 3 within a specific irradiation angle range θ corresponding to the upward and one side of the moving object 2.

[0032] Specifically, as shown in Figure 3(a), if there are points in the laser sensor 3's point cloud data D within a specific irradiation angle range θ corresponding to above and one side of the moving object 2, it is determined that the moving object 2 is indoors. As shown in Figure 3(b), if there are no points in the laser sensor 3's point cloud data D within a specific irradiation angle range θ corresponding to above and one side of the moving object 2, it is determined that the moving object 2 is outdoors.

[0033] The average distance calculation unit 15 calculates the average distance from the laser sensor 3 to the objects 6 surrounding the moving object 2, based on the point cloud data D of the laser sensor 3 acquired by the point cloud data acquisition unit 11.

[0034] The voxel size determination unit 16 determines the grid size of voxels V based on the determination result of the indoor / outdoor determination unit 14 and the calculation result of the average distance calculation unit 15. In other words, the voxel size determination unit 16 determines the grid size of voxels V according to the environmental conditions surrounding the moving body 2.

[0035] Indoors, structures such as shelves are placed. Outdoors, structures such as buildings are placed. Therefore, the size of structures placed indoors is often smaller than the size of structures placed outdoors. Consequently, when mobile object 2 is indoors, a smaller grid size for voxel V is desirable. When mobile object 2 is outdoors, a larger grid size for voxel V is desirable.

[0036] Therefore, when the indoor / outdoor determination unit 14 determines that the mobile body 2 is outdoors, the voxel size determination unit 16 increases the grid size of voxels V compared to when the indoor / outdoor determination unit 14 determines that the mobile body 2 is indoors.

[0037] Furthermore, as shown in Figure 4(a), when the distance from the laser sensor 3 to the object 6 is short, the object 6 is detected by the laser sensor 3 with a fine particle size. On the other hand, as shown in Figure 4(b), when the distance from the laser sensor 3 to the object 6 is long, the object 6 is detected by the laser sensor 3 with a coarse particle size.

[0038] Therefore, when the distance from the laser sensor 3 to the object 6 is short, a smaller grid size for voxel V is desirable. When the distance from the laser sensor 3 to the object 6 is long, a larger grid size for voxel V is desirable.

[0039] Therefore, as shown in Figure 5, the voxel size determination unit 16 increases the grid size of the voxels V as the distance from the laser sensor 3 to the object 6 increases.

[0040] Figure 6 is a flowchart showing the procedure for voxel size setting processing performed by the controller 5. This process is performed by the point cloud data acquisition unit 11, the indoor / outdoor determination unit 14, the average distance calculation unit 15, and the voxel size determination unit 16. This process is executed when the automatic driving of the mobile body 2 is instructed to begin.

[0041] In Figure 6, the controller 5 first acquires point cloud data D from the laser sensor 3 (procedure S101). Then, based on the point cloud data D from the laser sensor 3, the controller 5 recognizes the detection status of objects 6 present around the moving object 2 (procedure S102). The detection status of objects 6 includes the distribution of objects 6 and the distance to objects 6.

[0042] Next, the controller 5 determines whether the moving object 2 is indoors based on the distribution of the objects 6 (procedure S103). If the controller 5 determines that the moving object 2 is indoors, it selects an indoor determination value as the indoor / outdoor determination value (procedure S104). The indoor determination value is a fixed value when the grid size of voxel V is smaller than the specified value R, as shown in Figure 7.

[0043] When the controller 5 determines that the mobile object 2 is outdoors rather than indoors, it selects the outdoor determination value as the indoor / outdoor determination value (procedure S105). The outdoor determination value is a fixed value when the grid size of voxel V is greater than the specified value R, as shown in Figure 7. The outdoor determination value is a value greater than the indoor determination value. Note that when the grid size of voxel V is equal to the specified value R, either the indoor determination value or the outdoor determination value may be selected as the indoor / outdoor determination value.

[0044] After performing procedure S104 or S105, the controller 5 calculates the average distance from the laser sensor 3 to object 6 (hereinafter simply referred to as the average distance) based on the distance to object 6 (procedure S106).

[0045] Next, the controller 5 calculates and determines the grid size of voxel V using the indoor / outdoor determination value and the average distance (procedure S107). As shown in Figure 7, the grid size of voxel V corresponding to the average distance is predetermined. The grid size of voxel V is obtained by adding the indoor / outdoor determination value to the average distance, as shown in the following formula. Note that α is a constant. Grid size = (average distance × α) + indoor / outdoor determination value

[0046] Alternatively, the grid size of voxel V may be calculated by multiplying the average distance value by the indoor / outdoor determination value, as shown in the following formula. Note that α is a constant. Grid size = average distance × indoor / outdoor determination value × α

[0047] After executing step S107, the controller 5 executes step S101 again. As a result, the grid size of voxel V is set sequentially according to the environmental conditions surrounding the mobile body 2.

[0048] Here, the point cloud data acquisition unit 11 executes procedures S101 and S102. The indoor / outdoor determination unit 14 executes procedures S103 to S105. The average distance calculation unit 15 executes procedure S106. The voxel size determination unit 16 executes procedure S107.

[0049] When the voxel division unit 12 divides the point cloud data D of the laser sensor 3 into multiple voxels V, it divides the point cloud data D into voxels V of the grid size obtained in step S107 to generate voxel-level point cloud data Dv.

[0050] As described above, in this embodiment, the laser sensor 3 irradiates the moving body 2 with a laser L, and by receiving the reflected light of the laser L, objects 6 present around the moving body 2 are detected, and point cloud data D is output. The point cloud data D from the laser sensor 3 is then divided into multiple voxels V to generate voxel-level point cloud data Dv, and the self-position of the moving body 2 is estimated based on this voxel-level point cloud data Dv. At this time, the grid size of the voxels V is determined according to the environmental conditions around the moving body 2, and the point cloud data D from the laser sensor 3 is divided into voxels V of that grid size. Therefore, an appropriate voxel size is set according to the environmental conditions around the moving body 2. As a result, the accuracy of the self-position estimation of the moving body 2 is improved.

[0051] Furthermore, in this embodiment, it is determined whether the environment in which the mobile body 2 exists is indoors or outdoors. When it is determined that the mobile body 2 is outdoors, the grid size of voxel V is larger than when it is determined that the mobile body 2 is indoors. The size of an object 6 that exists indoors is often smaller than the size of an object 6 that exists outdoors. Therefore, when the mobile body 2 is outdoors, the grid size of voxel V is made larger than when the mobile body 2 is indoors, so that an appropriate voxel size is set regardless of whether the environment in which the mobile body 2 exists is indoors or outdoors.

[0052] Furthermore, in this embodiment, the presence or absence of point clouds within a specific irradiation angle range θ in the point cloud data D of the laser sensor 3 determines whether the environment in which the moving object 2 exists is indoors or outdoors. Thus, the presence or absence of points in the environment in which the moving object 2 exists is determined using the point cloud data D of the laser sensor 3. Therefore, it is not necessary to use a dedicated sensor other than the laser sensor 3.

[0053] Furthermore, in this embodiment, the grid size of voxel V increases as the distance from the laser sensor 3 to the object 6 increases. When the object 6 is close to the laser sensor 3, the object 6 is detected by the laser sensor 3 with fine granularity, but as the object 6 moves further away from the laser sensor 3, the detection granularity of the object 6 by the laser sensor 3 becomes coarser. Therefore, by increasing the grid size of voxel V as the distance from the laser sensor 3 to the object 6 increases, an appropriate voxel size is set regardless of the distance from the laser sensor 3 to the object 6.

[0054] Furthermore, in this embodiment, the grid size of voxel V increases as the average distance from the laser sensor 3 to the object 6 increases. Since the average distance from the laser sensor 3 to the object 6 is calculated in this way, an even more appropriate voxel size can be set.

[0055] It should be noted that the present invention is not limited to the embodiments described above. For example, in the above embodiments, it is determined whether the environment in which the moving object 2 is located is indoors or outdoors based on the presence or absence of a point cloud in a specific irradiation angle range θ in the point cloud data D of the laser sensor 3, but the invention is not limited to such a form. For example, the area around the moving object 2 may be imaged using a camera, and it may be determined whether the environment in which the moving object 2 is located is indoors or outdoors based on the image data from the camera.

[0056] Furthermore, in the above embodiment, the average distance from the laser sensor 3 to the object 6 is calculated, and the grid size of the voxel V is determined according to this average distance; however, the embodiment is not limited to this configuration. For example, the variance (dispersion) or minimum value of the distance from the laser sensor 3 to the object 6 may be calculated, and the grid size of the voxel V may be determined according to these calculated values.

[0057] Furthermore, in the above embodiment, the grid size of the voxel V is determined based on the environment in which the moving object 2 exists (indoors or outdoors) and the distance from the laser sensor 3 to the object 6, but the embodiment is not limited to this. The grid size of the voxel V may be determined based on either the environment in which the moving object 2 exists or the distance from the laser sensor 3 to the object 6.

[0058] Furthermore, in the above embodiment, a laser sensor 3 is used to detect the distance to an object 6 present around the moving body 2. However, if the grid size of the voxel V is to be determined based only on the environment in which the moving body 2 exists, a laser sensor that simply detects the presence or absence of an object 6 present around the moving body 2 may be used. [Explanation of Symbols]

[0059] 1...Self-position estimation device, 2...Moving object, 3...Laser sensor (laser object detection unit), 6...Object, 12...Voxel division unit, 13...Self-position estimation unit, 14...Indoor / outdoor determination unit, 15...Average distance calculation unit, 16...Voxel size determination unit, D...Point cloud data, Dv...Point cloud data, V...Voxel, L...Laser, θ...Specific irradiation angle range.

Claims

1. In a self-position estimation device that estimates the self-position of a moving object, A laser object detection unit irradiates a laser around the moving object, receives the reflected laser light, detects the distance to objects present around the moving object, and outputs point cloud data as detection data. A voxel division unit divides the point cloud data from the laser object detection unit into multiple voxels to generate point cloud data on a voxel-by-voxel basis, A self-position estimation unit estimates the self-position of the moving object based on the point cloud data of the voxel units generated by the voxel division unit, A voxel size determination unit that determines the grid size of the voxels according to the environmental conditions surrounding the moving body, An indoor / outdoor determination unit that determines whether the environment in which the moving object exists is indoors or outdoors, The system includes an average distance calculation unit that calculates the average distance from the laser object detection unit to the object, The voxel size determination unit determines the grid size of the voxels based on the determination result by the indoor / outdoor determination unit as to whether the environment in which the moving object exists is indoors or outdoors, and the calculation result of the average distance from the laser object detection unit to the object by the average distance calculation unit. The voxel division unit is a self-position estimation device that divides the point cloud data from the laser object detection unit into voxels of a grid size determined by the voxel size determination unit.

2. The self-position estimation device according to Claim 1, wherein the voxel size determination unit increases the grid size of the voxels when the indoor / outdoor determination unit determines that the moving body is outdoors, compared to when the indoor / outdoor determination unit determines that the moving body is indoors.

3. The self-position estimation device according to claim 2, wherein the indoor / outdoor determination unit determines whether the environment in which the moving object exists is indoors or outdoors based on the presence or absence of a point cloud in a specific irradiation angle range in the point cloud data of the laser object detection unit.

4. The self-position estimation device according to any one of Claims 1 to 3, wherein the voxel size determination unit increases the grid size of the voxels as the distance from the laser object detection unit to the object increases.

5. The self-position estimation device according to claim 4, wherein the voxel size determination unit increases the grid size of the voxels as the average value of the distance calculated by the average distance calculation unit increases.

Images