Self position estimation system, self position estimation device, self position estimation method, and program

Abstract

To provide a highly accurate self position estimation technology using a far distance point group.SOLUTION: A self position estimation system includes reference environment point group storage means for storing a reference environment point group for self position estimation, point group acquisition means for acquiring a near-field point group of a wide field of view and a far-field point group that is farther than the near-field point group and is a near-field of view, rough estimation means for roughly estimating a self position by positioning the near-field point group to the reference environment point group, and precise estimation means for precisely estimating the self position by positioning the far-field point group to the reference environment point group with a rough estimation result by the rough estimation means as an initial condition.SELECTED DRAWING: Figure 1

Description

[Technical field]

[0001] The present disclosure relates to a self-location estimation system, a self-location estimation device, a self-location estimation method, and a program. [Background technology]

[0002] Patent Document 1 discloses an autonomous vehicle equipped with a sensor unit including a first LiDAR (Light Detection And Ranging) and a second LiDAR. Specifically, the first LiDAR is used to detect an object about 100 meters away from the vehicle. The first LiDAR is configured to measure distances with a FoV (Field of View) having a horizontal angle of 360 degrees and a vertical angle of 20 degrees. Meanwhile, the second LiDAR is used to detect an object about 300 meters away from the vehicle. The second LiDAR is configured to measure distances with a FoV having a horizontal angle of 8 degrees and a vertical angle of 15 degrees. The vehicle autonomously drives based on the point cloud output from the first LiDAR and the point cloud output from the second LiDAR. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2018-049014 A Summary of the Invention [Problem to be solved by the invention]

[0004] By the way, in the field of autonomous driving technology for automobiles, SLAM (Simultaneous Localization and Mapping) using LiDAR point clouds is becoming practical. In the field of railways, autonomous driving using SLAM is also expected to be put to practical use.

[0005] For example, to realize autonomous driving of a train traveling at over 100 kilometers per hour, it is typically necessary to monitor more than 500 meters ahead of the train, taking into account the braking distance required to detect obstacles ahead and stop safely. In order to monitor such long distances, it is necessary to use the aforementioned SLAM to reconstruct a 3D map of the environment ahead of the train with high accuracy over long distances.

[0006] To reconstruct a three-dimensional map with high accuracy using SLAM, it is essential to accurately correct the position of the LiDAR point cloud acquired from the LiDAR device mounted on the train according to the movement of the train. This is because the three-dimensional map is reconstructed by repeatedly superimposing LiDAR point clouds whose positions have been appropriately corrected. In addition, the coordinate values ​​of the LiDAR point cloud output from the LiDAR device are expressed in the LiDAR coordinate system fixed to the LiDAR device, not in the map coordinate system of the three-dimensional map. In order to realize the above-mentioned high-accuracy position correction, the position of the LiDAR coordinate system in the map coordinate system of the three-dimensional map must be estimated with high accuracy. Here, estimating the position of the LiDAR coordinate system is generally called self-location estimation. In addition, the position of the LiDAR coordinate system in the map coordinate system of the three-dimensional map is composed of three-axis translation components and three-axis rotation components.

[0007] However, due to the characteristics of the LiDAR device, the density of the LiDAR point cloud far from the LiDAR device is relatively low, so it is quite difficult to estimate the self-location with high accuracy using the LiDAR point cloud far from the LiDAR device.

[0008] In response to this, it is conceivable to increase the density of the LiDAR point cloud far from the LiDAR device by narrowing the FoV of the LiDAR device. However, in this case, since the LiDAR point cloud becomes an extremely small mass, there is a risk that the registration calculation for aligning the LiDAR point cloud with the reference point cloud will converge to an incorrect solution, which will make it difficult to estimate the self-location with high accuracy, as in the above case.

[0009] For these reasons, it is not easy to use SLAM to reconstruct a 3D map of the environment ahead of the train with high accuracy over a long distance, and therefore it is not possible to adequately monitor the area ahead of the train over a long distance.

[0010] Therefore, an object of the present disclosure is to provide a highly accurate self-location estimation technology using a long-distance point cloud. [Means for solving the problem]

[0011] A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, A self-location estimation system is provided.

[0012] A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, A self-location estimation device is provided.

[0013] storing a reference environment point cloud for self-location estimation; Acquire a near point cloud with a wide field of view and a far point cloud with a narrow field of view that is farther away than the near point cloud; Roughly estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; Using a result of the rough estimation as an initial condition, the far-distance point cloud is aligned with the reference environment point cloud to precisely estimate the self-location. A method for self-location estimation is provided. Effect of the Invention

[0014] According to the present disclosure, a highly accurate self-location estimation technique using a long-distance point cloud is realized. [Brief description of the drawings]

[0015] [Figure 1] FIG. 1 is a block diagram of a self-location estimation system. [Diagram 2] FIG. 2 is a block diagram of a train. [Diagram 3] FIG. 1 is a planar distribution diagram of distance measurement points. [Figure 4] FIG. [Diagram 5] 1 is a control flow of an environment point cloud generating device. [Figure 6] FIG. 2 is a block diagram of a train. [Figure 7] FIG. 1 is a planar distribution diagram of distance measurement points. [Figure 8] FIG. 1 is an explanatory diagram regarding the continuity of a point cloud. [Figure 9] FIG. 2 is a block diagram of a train. [Figure 10] FIG. 2 is a data structure diagram of a structure position database. [Figure 11] FIG. 1 is a planar distribution diagram of distance measurement points. [Figure 12] FIG. 13 is a diagram showing the relationship between a far-distance point cloud and a contributing point cloud. [Figure 13] FIG. 2 is a block diagram of an environment point cloud generator. [Figure 14] FIG. 13 is a diagram showing the relationship between a reference environment point group and a partial reference environment point group. [Figure 15] 1 is a diagram showing a case where the processing circuit of the environment point cloud generating device is configured with a processor and a memory. [Figure 16]1 is a diagram showing a case where the processing circuit of the environment point cloud generating device is configured with dedicated hardware. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0016] (Summary of the Disclosure) The present disclosure is outlined as follows: Figure 1 is a block diagram of a self-location estimation system.

[0017] As shown in FIG. 1, the self-location estimation system 100 includes a reference environment point cloud storage means 101, a point cloud acquisition means 102, a rough estimation means 103, and a precise estimation means 104.

[0018] The reference environment point group storage means 101 stores a reference environment point group for self-location estimation.

[0019] The point cloud acquisition means 102 acquires a close-distance point cloud with a wide field of view, and a long-distance point cloud that is farther away than the close-distance point cloud and has a narrower field of view.

[0020] The rough estimation means 103 roughly estimates the self-location by aligning the close distance point cloud with the reference environment point cloud.

[0021] A precise estimation means 104 precisely estimates the self-location by aligning the far-distance point cloud with the reference environment point cloud, using the rough estimation result by the rough estimation means 103 as an initial condition.

[0022] According to the above configuration, a highly accurate self-location estimation technique using a long-distance point cloud is realized.

[0023] (First embodiment) Next, a first embodiment of the present disclosure will be described. Fig. 2 is a block diagram of a train. As shown in Fig. 2, a train 1 runs along a railway track 2.

[0024] The train 1 includes a short-range LiDAR device 3, a long-range LiDAR device 4, an IMU (Inertial Measurement Unit) 5, an environmental point cloud generator 6, an environmental point cloud storage device 7, a forward monitoring device 8, a driving device 9, a braking device 10, and a plurality of wheels 11.

[0025] The drive device 9 includes a motor and a motor driver. The drive device 9 drives the wheels 11 in accordance with a control signal from the forward monitoring device 8.

[0026] The braking device 10 is composed of a brake and a hydraulic circuit. The braking device 10 brakes the wheels 11 in accordance with a control signal from the forward monitoring device 8.

[0027] The railway track 2 includes a ballast, a plurality of sleepers 2a arranged at predetermined intervals on the ballast, and two rails 2b supported by the plurality of sleepers 2a.

[0028] The short-range LiDAR device 3 and the long-range LiDAR device 4 are typically LiDAR devices mounted on the leading car of the train 1.

[0029] The LiDAR device is a specific example of a sensing means for sensing the area ahead in the traveling direction of the train 1 to generate a point cloud. The LiDAR device of this embodiment is a direct ToF (Time of Flight) type. That is, the LiDAR device emits laser light ahead in the traveling direction of the train 1 and measures the time required to receive the reflected light, thereby generating a point cloud ahead in the traveling direction of the train 1. However, instead of this, the LiDAR device may be an FMCW (Frequency Modulated Continuous Wave) type that generates the above-mentioned point cloud based on the frequency difference between the laser light emitted ahead in the traveling direction of the train 1 and the reflected light. In addition, the LiDAR device may be an indirect ToF type that generates the above-mentioned point cloud based on the phase difference between the laser light emitted ahead in the traveling direction of the train 1 and the reflected light. The LiDAR device outputs the generated point cloud to the environment point cloud generating device 6.

[0030] The sensing means is not limited to a LiDAR device. Any device capable of sensing the area ahead of the train 1 in the traveling direction may be adopted as the sensing means. For example, the sensing means may be configured to generate a point cloud using a radar device (Radio Detection And Ranging), an ultrasonic sensor, a stereo camera, or a combination of these. The sensing means may also be configured to generate a point cloud using Structure from Motion (SfM) from a plurality of two-dimensional images obtained by capturing an image of the area ahead of the train 1 in the traveling direction.

[0031] The short-range LiDAR device 3 is a LiDAR device for short-range detection with a detection distance of 20 meters to 200 meters. The short-range LiDAR device 3 outputs a point cloud generated while the train 1 is running to the environmental point cloud generating device 6 as a short-range point cloud.

[0032] The long-range LiDAR device 4 is a LiDAR device for long-range detection with a detection distance of 400 meters to 1000 meters. The long-range LiDAR device 4 outputs a point cloud generated while the train 1 is running to the environmental point cloud generating device 6 as a long-range point cloud.

[0033] Please refer to Figures 3 and 4. Figures 3 and 4 are explanatory diagrams of the measurement field of view of the LiDAR device. In Figure 3, a plurality of measurement points constituting the point cloud are drawn in a planar view. Figure 4 shows the point cloud ahead in the traveling direction of the train 1.

[0034] As shown in FIG. 3, the horizontal angle θ3 of the ranging field 3a of the short-distance LiDAR device 3 is typically set to 120 degrees. That is, the short-distance LiDAR device 3 is set to measure distances with a relatively wide ranging field 3a. In contrast, the horizontal angle θ4 of the ranging field 4a of the long-distance LiDAR device 4 is typically set to 5 degrees. That is, the long-distance LiDAR device 4 is configured to measure distances with a relatively narrow ranging field 4a. Similarly, as shown in FIG. 4, the vertical angle of the ranging field 3a of the short-distance LiDAR device 3 is set to be wider than the vertical angle of the ranging field 4a of the long-distance LiDAR device 4.

[0035] The line of sight of the short-range LiDAR device 3 is set to be ahead in the traveling direction of the train 1 and parallel to the longitudinal direction of the track 2. Similarly, the line of sight of the long-range LiDAR device 4 is set to be ahead in the traveling direction of the train 1 and parallel to the longitudinal direction of the track 2. Therefore, as shown in Figures 3 and 4, the ranging field of view 3a of the short-range LiDAR device 3 and the ranging field of view 4a of the long-range LiDAR device 4 overlap each other. In addition, the ranging field of view 4a of the long-range LiDAR device 4 is a field of view inside the ranging field of view 3a of the short-range LiDAR device 3.

[0036] 2, the IMU 5 detects the three-axis acceleration and three-axis angular velocity of the train 1. The IMU 5 outputs the detection results to the environment point cloud generating device 6.

[0037] The environment point cloud generating device 6 reconstructs an environment point cloud ahead in the traveling direction of the train 1 based on the short-distance point cloud acquired from the short-distance LiDAR device 3 and the long-distance point cloud acquired from the long-distance LiDAR device 4. The environment point cloud generating device 6 stores the reconstructed environment point cloud in the environment point cloud storage device 7. The forward monitoring device 8 detects obstacles ahead in the traveling direction of the train 1 based on the environment point cloud stored in the environment point cloud storage device 7. The environment point cloud generating device 6, the environment point cloud storage device 7, and the forward monitoring device 8 will be described in detail below.

[0038] The environment point cloud generating device 6 includes a point cloud acquisition unit 20, a close-range point cloud memory unit 21, a long-range point cloud memory unit 22, a rough estimation unit 23, a self-position memory unit 24, a precise estimation unit 25, and an environment point cloud generating unit 26.

[0039] The point cloud acquisition unit 20 is a specific example of a point cloud acquisition means. The point cloud acquisition unit 20 acquires a short-distance point cloud from the short-distance LiDAR device 3. The point cloud acquisition unit 20 stores the acquired short-distance point cloud in the short-distance point cloud storage unit 21 while performing movement compensation based on the detection result of the IMU 5.

[0040] Similarly, the point cloud acquisition unit 20 acquires a long-distance point cloud from the long-distance LiDAR device 4. The point cloud acquisition unit 20 stores the acquired long-distance point cloud in the long-distance point cloud storage unit 22 while performing movement compensation based on the detection result of the IMU 5.

[0041] 3 shows a close-distance point cloud P and a long-distance point cloud Q. The close-distance point cloud P is composed of multiple ranging points p. The long-distance point cloud Q is composed of multiple ranging points q. Each ranging point p and each ranging point q is data composed of coordinate data, brightness data, ranging time data, etc. in a LiDAR coordinate system fixed to the close-distance LiDAR device 3.

[0042] As shown in Fig. 3, multiple ranging points p constituting the close-distance point cloud P are distributed in a range of 20 meters to 200 meters from the train 1. In contrast, multiple ranging points q constituting the long-distance point cloud Q are distributed in a range of 400 meters to 600 meters from the train 1. As shown in Fig. 3, although the long-distance point cloud Q is far from the train 1, it has a sufficiently high point cloud density because the ranging field of view 4a of the long-distance LiDAR device 4 is set relatively narrow.

[0043] Here, the environment point cloud storage device 7 will be described. The environment point cloud storage device 7 is a specific example of a reference environment point cloud storage means. As shown in FIG. 2, the environment point cloud storage device 7 stores a reference environment point cloud 30 and a latest environment point cloud 31. The reference environment point cloud 30 is a point cloud of the surrounding environment of the track 2 on which the train 1 runs. The reference environment point cloud 30 is a point cloud generated at a certain point in the past. The reference environment point cloud 30 is typically generated by triggering the running of a preceding train or an oncoming train. The latest environment point cloud 31 is a point cloud of the surrounding environment of the track 2 generated in real time while the train 1 runs.

[0044] Returning to the description of the environment point cloud generating device 6, the rough estimation unit 23 is a specific example of a rough estimation means. The rough estimation unit 23 roughly estimates the self-location by aligning the close-distance point cloud P stored in the close-distance point cloud storage unit 21 with the reference environment point cloud 30. The rough estimation unit 23 stores the roughly estimated coarse self-location in the self-location storage unit 24. The rough self-location is a specific example of a rough estimation result. Here, the self-location means the position of the LiDAR coordinate system of the close-distance LiDAR device 3 in the reference coordinate system of the reference environment point cloud 30. The position of the LiDAR coordinate system of the close-distance LiDAR device 3 in the reference coordinate system of the reference environment point cloud 30 is typically composed of three-axis translation components and three-axis rotation components. For example, ICP (Iterative Closest Point) and NDT (Normal Distributions Transform) can be used as the above-mentioned alignment method. As described above, the field of view 3a of the short-distance LiDAR device 3 when generating the short-distance point cloud P is set to a relatively wide field of view, so the convergence of the alignment calculation for aligning the short-distance point cloud P with the reference environment point cloud 30 can be said to be good. However, the accuracy of the coarse self-position estimated by aligning the short-distance point cloud P with the reference environment point cloud 30 is somewhat poor. In particular, a slight angle error remains in the rotational component of the coarse self-position. This angle error has a significant adverse effect when reconstructing the latest environment point cloud 31 by superimposing multiple sets of long-distance point clouds, as will be described later.

[0045] The precise estimation unit 25 is a specific example of a precise estimation means. The precise estimation unit 25 precisely estimates the self-location by aligning the far-distance point cloud Q with the reference environment point cloud 30, using the coarse self-location stored in the self-location storage unit 24 as an initial condition. The self-location storage unit 24 stores the precisely estimated precise self-location in the self-location storage unit 24. In other words, the precise estimation unit 25 corrects the coarse self-location by precise estimation. The precise estimation unit 25 stores the precise self-location, which is the corrected coarse self-location, in the self-location storage unit 24.

[0046] As described above, the alignment calculation such as ICP and NDT has a property that the convergence depends on the quality of the initial condition of the alignment calculation. In particular, when the point cloud density is originally low like the long distance point cloud Q and the ranging field of view 4a is set to a relatively narrow field of view, the convergence depends greatly on the quality of the initial condition. In contrast, in this embodiment, since the rough self-position calculated by the rough estimation unit 23 is used as the initial condition, it can be said that the initial condition of the alignment calculation is sufficient, and therefore the convergence of the alignment calculation by the precise estimation unit 25 is sufficiently good.

[0047] The environment point cloud generation unit 26 generates the latest environment point cloud 31 based on the precise self-position stored in the self-position storage unit 24 and the far-distance point cloud Q. That is, the environment point cloud generation unit 26 generates the latest environment point cloud 31 by superimposing multiple sets of far-distance point clouds Q in the reference coordinate system while shifting them based on the precise self-position corresponding to the far-distance point cloud Q.

[0048] As mentioned above, this process of estimating self-position by aligning the long-distance point cloud Q with the reference environment point cloud 30, or overlaying multiple sets of long-distance point clouds Q to generate the latest environment point cloud 31, is called SLAM.

[0049] The forward monitoring device 8 includes an obstacle determining unit 40 and a vehicle control unit 41.

[0050] The obstacle determination unit 40 determines the presence or absence of an obstacle ahead in the traveling direction of the train 1 based on the latest environment point cloud 31 stored in the environment point cloud storage device 7. Specifically, the obstacle determination unit 40 may extract a point cloud that is the difference between the reference environment point cloud 30 and the latest environment point cloud 31, and determine the presence or absence of an obstacle based on the extracted point cloud. Alternatively, the obstacle determination unit 40 may determine the presence or absence of an obstacle ahead in the traveling direction of the train 1 by identifying a swept volume through which the train 1 passes based on the latest environment point cloud 31, and detecting an object within the swept volume using PointNet or the like.

[0051] When the obstacle determination unit 40 detects an obstacle, the vehicle control unit 41 outputs a deceleration signal to the braking device 10. As a result, the vehicle control unit 41 stops the train 1 before the train 1 collides with the obstacle.

[0052] Next, a description will be given of the control flow of the environment point cloud generating device 6. Fig. 5 shows the control flow of the environment point cloud generating device 6. Here, it is assumed that SLAM is performed while a train 1 is traveling along a track 2.

[0053] First, the point cloud acquisition unit 20 acquires a close point cloud P and a far point cloud Q (S100). Next, the coarse estimation unit 23 roughly estimates the self-location by aligning the close point cloud P with the reference environment point cloud 30 (S110). Next, the precise estimation unit 25 precisely estimates the self-location by aligning the far point cloud Q with the reference environment point cloud 30 using the coarse self-location as an initial condition (S120). Next, the environment point cloud generation unit 26 generates the latest environment point cloud 31 based on the far point cloud Q and the precise self-location (S130).

[0054] The first embodiment of the present disclosure has been described above, and the first embodiment has the following features.

[0055] The environment point cloud generating device 6 is a specific example of a self-location estimation system and a self-location estimation device. As shown in FIG. 2, the environment point cloud generating device 6 includes an environment point cloud storage device 7 (reference environment point cloud storage means), a point cloud acquisition unit 20 (point cloud acquisition means), a rough estimation unit 23 (rough estimation means), and a precise estimation unit 25 (precise estimation means). The environment point cloud storage device 7 stores a reference environment point cloud 30 for self-location estimation. The point cloud acquisition unit 20 acquires a close-distance point cloud P with a wide field of view and a far-distance point cloud Q with a narrow field of view that is farther than the close-distance point cloud. The rough estimation unit 23 roughly estimates the self-location by aligning the close-distance point cloud P with the reference environment point cloud 30. The precise estimation unit 25 precisely estimates the self-location by aligning the far-distance point cloud Q with the reference environment point cloud 30, using the rough self-location as a rough estimation result by the rough estimation unit 23 as an initial condition. According to the above configuration, a highly accurate self-location estimation technique using the far-distance point cloud Q is realized.

[0056] A wide field of view is an example of a first field of view. A narrow field of view is an example of a second field of view that is narrower than the first field of view.

[0057] The near point cloud is a specific example of a first point cloud, and the far point cloud is a specific example of a second point cloud that is farther away than the first point cloud.

[0058] As shown in FIG. 3, the close distance point cloud P and the long distance point cloud Q are both point clouds obtained by measuring the distance ahead in the traveling direction of the leading car (car) of the train 1.

[0059] As shown in FIGS. 3 and 4, the distance measurement field of view 3a of the close distance point cloud P and the distance measurement field of view 4a of the far distance point cloud Q overlap each other.

[0060] Second embodiment Next, a second embodiment will be described. Below, the differences between the second embodiment and the first embodiment will be mainly described, and overlapping descriptions will be omitted.

[0061] Please refer to Fig. 3 again. As shown in Fig. 3, the long-distance point cloud Q has a low point cloud density to begin with, and therefore has a tendency to easily generate isolated points. Such isolated points act as noise in the precise estimation of the self-position by the precise estimation unit 25, and may be a factor in reducing the estimation accuracy of the precise estimation. Therefore, in this embodiment, ranging points that do not contribute to the precise estimation of the self-position by the precise estimation unit 25 are removed from the long-distance point cloud Q. Specifically, this is as follows.

[0062] FIG. 6 is a block diagram of the train 1. As shown in FIG. 6, the precise estimation unit 25 extracts a contribution point group 50 that contributes to the precise estimation from the long-distance point group Q. Then, the precise estimation unit 25 precisely estimates the self-position by aligning the contribution point group 50 with the reference environment point group 30. This can further improve the estimation accuracy of the precise estimation. Furthermore, by aligning the contribution point group 50, which has a smaller number of ranging points than the long-distance point group Q, with the reference environment point group 30 instead of the long-distance point group Q, the amount of calculation required for the alignment operation can be simply reduced.

[0063] In this embodiment, the contributing point group 50 is a "point group having continuity of a predetermined distance or more." Please refer to Fig. 7. Fig. 7 is a distribution diagram of ranging points. As shown in Fig. 7, ranging points q1 to q21 are scattered randomly, but ranging points q22 to q34 are arranged in a somewhat continuous fashion. Similarly, ranging points q35 to q48 are arranged in a somewhat continuous fashion.

[0064] Here, the "point group having continuity" will be described with reference to FIG. 8. FIG. 8 is an explanatory diagram of continuity. FIG. 8 shows distance measurement point q31, distance measurement point q32, distance measurement point q33, distance measurement point q34, and distance measurement point q20. When a circle C31 having a predetermined diameter centered on distance measurement point q31 is drawn, distance measurement point q32 is located within the circle C31. Therefore, it can be said that distance measurement point q31 and distance measurement point q32 are close to each other and have continuity. Similarly, when a circle C32 having a predetermined diameter centered on distance measurement point q32 is drawn, distance measurement point q33 is located within the circle C32. Therefore, it can be said that distance measurement point q32 and distance measurement point q33 are close to each other and have continuity. Similarly, when a circle C33 having a predetermined diameter centered on distance measurement point q33 is drawn, distance measurement point q34 is located within the circle C33. Therefore, it can be said that ranging points q33 and q34 are adjacent to each other and have continuity. In contrast, when a circle C34 of a specified diameter is drawn with ranging point q34 at its center, ranging point q20 is not located within said circle C34. Therefore, it can be said that ranging points q34 and q20 are not adjacent to each other and do not have continuity. As a result, ranging points q31, q32, q33, and q34 correspond to a "point group having continuity."

[0065] Next, the "point group having continuity of a predetermined distance or more" will be described with reference to FIG. 8. As described above, the distance measurement point q31, the distance measurement point q32, the distance measurement point q33, and the distance measurement point q34 correspond to a point group having continuity. A single broken line L is drawn to connect the distance measurement point q31, the distance measurement point q32, the distance measurement point q33, and the distance measurement point q34 to each other. Specifically, the order in which the distance measurement point q31, the distance measurement point q32, the distance measurement point q33, and the distance measurement point q34 are connected is determined so that the length of the broken line L is the shortest. Then, when the length of the broken line L is the predetermined distance or more, the distance measurement point q31, the distance measurement point q32, the distance measurement point q33, and the distance measurement point q34 correspond to the "point group having continuity of a predetermined distance or more". Here, the "length of the broken line L" corresponds to the sum of the length of the line segment d1 connecting the ranging points q31 and q32, the length of the line segment d2 connecting the ranging points q32 and q33, and the length of the line segment d3 connecting the ranging points q33 and q34. In FIG. 8, the elevation values ​​(Z values) of the ranging points q31, q32, q33, and q34 are not taken into consideration, and the "point cloud having continuity of a predetermined distance or more" is defined as all of these ranging points existing on the same horizontal plane. However, in reality, it is rare that all of the ranging points q31, q32, q33, and q34 exist on the same horizontal plane, and the elevation values ​​(Z values) of the ranging points q31, q32, q33, and q34 are different from one another. Therefore, whether or not a "cloud of points having continuity" actually corresponds to a "cloud of points having continuity of a specified distance or more" will require a determination based on a three-dimensional definition of the aforementioned broken line L.

[0066] The method of extracting the contributing points 50 from the far-distance point cloud Q is not limited to the above. For example, a point cloud that is gathered at a certain point cloud density from the far-distance point cloud Q may be extracted as the contributing points 50. Also, the criterion for determining continuity may be adjusted by appropriately increasing or decreasing the diameter of the circle C shown in FIG. 8. Also, the contributing points 50 may be extracted from the far-distance point cloud Q by using a segmentation algorithm or a RANSAC algorithm.

[0067] The second embodiment of the present disclosure has been described above, and the above embodiment has the following features.

[0068] That is, the precise estimation unit 25 extracts a contribution point group 50 that contributes to the precise estimation from the long-distance point group Q. The precise estimation unit 25 precisely estimates the self-position by aligning the extracted contribution point group 50 with the reference environment point group 30. With the above configuration, the estimation accuracy of the precise estimation is further improved, and the amount of calculation for the alignment operation can be reduced.

[0069] Moreover, the contributing point group 50 is a point group having continuity of a predetermined distance or more. According to the above configuration, the contributing point group 50 is highly likely to be a point group corresponding to a structure that is likely to contribute to position estimation, such as a wall surface of a building, which is clearly different from a noise point group. Therefore, the estimation accuracy of the precise estimation by the precise estimation unit 25 can be further improved.

[0070] In addition, structures include buildings such as buildings and apartments that are built for people to live or gather, and structures built to make life more convenient, such as bridges, tunnels, dams, and steel towers.

[0071] Third embodiment Next, a third embodiment will be described. Below, the differences between the third embodiment and the second embodiment will be mainly described, and overlapping descriptions will be omitted.

[0072] In the second embodiment, the contributing point group 50 is a group of points having continuity of a predetermined distance or more.

[0073] In contrast, in this embodiment, the precise estimation unit 25 extracts a point group corresponding to buildings from the long-distance point cloud Q as the contributing point group 50. This is because the point group corresponding to buildings is connected to have an angle in a planar view, and therefore contributes greatly to precise estimation by the precise estimation unit 25.

[0074] FIG. 9 is a block diagram of the train 1. As shown in FIG. 9, in this embodiment, the environment point cloud generating device 6 further includes a structure position database storage unit 51 that stores a structure position database 52. The structure position database storage unit 51 is a specific example of a structure position database storage means. FIG. 10 shows the data structure of the structure position database 52. As shown in FIG. 10, the structure position database 52 is a database regarding the position information of a plurality of structures suitable for alignment calculation. The position information of each structure is expressed in a reference coordinate system. According to the structure position database 52, for example, the position information of structure No. 245 is (x245, y245, z245).

[0075] Then, the precise estimation unit 25 refers to the structure position database 52 to extract a point group corresponding to at least one structure from the long-distance point group Q as the contributing point group 50. Specifically, the precise estimation unit 25 calculates the positions of the multiple structures in the LiDAR coordinate system based on the positions of the multiple structures registered in the structure position database 52 and the rough self-position. Here, as shown in FIG. 11, for example, it is assumed that structure No. 241 and structure No. 249 exist near the long-distance point group Q. In this case, the precise estimation unit 25 draws a circle C241 of a predetermined diameter centered on the position of the structure No. 241, and extracts multiple ranging points existing within the circle C241 from the long-distance point group Q as the contributing point group 50. In the example of FIG. 11, the multiple ranging points existing within the circle C241 are ranging point q9, ranging point q16, ranging point q20, ranging point q22-ranging point q34, and the like. Similarly, in this case, the precise estimation unit 25 draws a circle C249 of a predetermined diameter centered on the position of the structure No. 249, and extracts multiple ranging points present within the circle C249 from the long-distance point cloud Q as the contributing point cloud 50. In the example of Fig. 11, the multiple ranging points present within the circle C249 are ranging point q10, ranging point q17, ranging point q35 to ranging point q48, etc.

[0076] Fig. 12 shows the far-distance point cloud Q and the contribution point cloud 50 extracted from the far-distance point cloud Q. The far-distance point cloud Q corresponds to the upper one of the two regions surrounded by the two-dot chain line in Fig. 12. The contribution point cloud 50 corresponds to the lower one of the two regions surrounded by the two-dot chain line in Fig. 12. As shown in Fig. 12, the contribution point cloud 50 has a number of measurement points that is fewer than the number of measurement points that constitute the far-distance point cloud Q.

[0077] The third embodiment of the present disclosure has been described above, and the above embodiment has the following features.

[0078] That is, the environment point cloud generating device 6 further includes a structure position database storage unit 51 that stores a structure position database 52 indicating the position information of a plurality of structures that contribute to the precise estimation. The precise estimation unit 25 extracts a point group corresponding to a plurality of structures from the long-distance point cloud Q as a contribution point group 50 by referring to the structure position database 52. This improves the estimation accuracy of the precise estimation. Furthermore, by aligning the contribution point group 50, which has a smaller number of ranging points than the long-distance point cloud Q, with the reference environment point cloud 30 instead of the long-distance point cloud Q, the amount of calculation for the alignment operation can be simply reduced. In addition, compared to the second embodiment, since no complicated calculation is required to extract the contribution point group 50 from the long-distance point cloud Q, the real-time information processing of the environment point cloud generating device 6 is improved.

[0079] (Fourth embodiment) Next, a fourth embodiment of the present disclosure will be described. Below, the differences between the fourth embodiment and the third embodiment will be mainly described, and overlapping descriptions will be omitted.

[0080] In the third embodiment, the precise estimation unit 25 precisely estimates its own position by aligning the contribution point group 50 with the reference environment point group 30 .

[0081] In contrast, in this embodiment, the number of multiple ranging points constituting the reference environment point group 30 is further reduced, thereby reducing the amount of calculation required for the alignment calculation when aligning the contribution point group 50 with the reference environment point group 30. That is, in this embodiment, just as the precise estimation unit 25 extracted a point group corresponding to buildings from the long-distance point group Q as the contribution point group 50 in the third embodiment, a point group corresponding to buildings is extracted from the reference environment point group 30 as a partial reference environment point group. Then, instead of aligning the long-distance point group Q with the reference environment point group 30, the precise estimation unit 25 aligns the contribution point group 50 with the partial reference environment point group. Specifically, this is as follows.

[0082] FIG. 13 is a block diagram of the environment point cloud generating device 6. As shown in FIG. 13, the environment point cloud generating device 6 further includes a partial reference environment point cloud generating unit 53 and a partial reference environment point cloud storage unit 54. The partial reference environment point cloud generating unit 53 generates a partial reference environment point cloud 55. Specifically, the partial reference environment point cloud generating unit 53 generates the partial reference environment point cloud 55 by deleting ranging points other than ranging points corresponding to a plurality of buildings from the long distance point cloud Q. At this time, the partial reference environment point cloud generating unit 53 may refer to a structure position database 52 stored in a structure position database storage unit 51. The partial reference environment point cloud generating unit 53 can determine whether each ranging point corresponds to any of a plurality of buildings by referring to the structure position database 52. The partial reference environment point cloud generating unit 53 stores the generated partial reference environment point cloud 55 in the partial reference environment point cloud storage unit 54. The partial reference environment point group storage unit 54 is a specific example of a partial reference environment point group storage means for storing the partial reference environment point group 55. FIG. 14 shows the reference environment point group 30 and the partial reference environment point group 55. The reference environment point group 30 corresponds to the upper area of ​​the two areas surrounded by the two-dot chain line in FIG. 14. The partial reference environment point group 55 corresponds to the lower area of ​​the two areas surrounded by the two-dot chain line in FIG. 14. As shown in FIG. 14, the partial reference environment point group 55 is a part of the reference environment point group 30. The partial reference environment point group 55 is composed of a point group related to buildings in the reference environment point group 30.

[0083] Then, the precise estimation unit 25 precisely estimates the self-location by aligning the contribution point group 50 with the partial reference environment point group 55 .

[0084] The fourth embodiment has been described above, and has the following features.

[0085] The environment point cloud generating device 6 further includes a partial reference environment point cloud 55 that stores a partial reference environment point cloud 55 obtained by deleting ranging points other than those corresponding to a plurality of buildings from the reference environment point cloud 30. The precise estimation unit 25 precisely estimates the self-position by aligning the contribution point group 50 with the partial reference environment point group 55. According to the above configuration, the amount of data of the point cloud to which the contribution point group 50 is aligned is reduced compared to the third embodiment, and thus the real-time information processing of the environment point cloud generating device 6 is further improved.

[0086] Next, the hardware configuration of the environment point cloud generating device 6 will be described. In the environment point cloud generating device 6, the point cloud acquisition unit 20, the rough estimation unit 23, the precise estimation unit 25, the environment point cloud generation unit 26, and the partial reference environment point cloud generation unit 53 are realized by processing circuits. The close point cloud storage unit 21, the long point cloud storage unit 22, the self-position storage unit 24, the structure position database storage unit 51, and the partial reference environment point cloud storage unit 54 are realized by storage circuits. The processing circuit may be a processor and memory that executes a program stored in a memory, or may be dedicated hardware.

[0087] FIG. 15 is a diagram showing an example of a case where the processing circuit of the environment point cloud generating device 6 is configured with a processor and a memory. When the processing circuit is configured with a processor 1000 and a memory 1001, each function of the processing circuit of the environment point cloud generating device 6 is realized by software, firmware, or a combination of software and firmware. The software or firmware is written as a program and stored in the memory 1001. In the processing circuit, each function is realized by the processor 1000 reading and executing the program stored in the memory 1001. That is, the processing circuit includes the memory 1001 for storing a program that results in the processing of the environment point cloud generating device 6 being executed. It can also be said that these programs cause a computer to execute the procedures and methods of the environment point cloud generating device 6.

[0088] Here, the processor 1000 may be a CPU (Central Processing Unit), a processing device, an arithmetic device, a microprocessor, a microcomputer, or a DSP (Digital Signal Processor), etc. Also, the memory 1001 may be, for example, a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable ROM), an EEPROM (Electrically EPROM), or other non-volatile or volatile semiconductor memory, a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, or a DVD (Digital Versatile Disc), etc.

[0089] Fig. 16 is a diagram showing an example in which the processing circuit of the environment point cloud generating device 6 is configured with dedicated hardware. When the processing circuit is configured with dedicated hardware, the processing circuit 1002 shown in Fig. 16 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination of these. Each function of the environment point cloud generating device 6 may be realized by the processing circuit 1002 on a function-by-function basis, or each function may be realized collectively by the processing circuit 1002.

[0090] It should be noted that the functions of the environment point cloud generating device 6 may be partially realized by dedicated hardware and partially realized by software or firmware. In this manner, the processing circuit can realize each of the above-mentioned functions by dedicated hardware, software, firmware, or a combination of these.

[0091] Although the present disclosure has been described above with reference to the embodiments, the present disclosure is not limited to the above-described embodiments. Various modifications that can be understood by a person skilled in the art can be made to the configuration and details of the present disclosure within the scope of the present disclosure.

[0092] That is, as shown in Fig. 2, the train 1 is equipped with a short-range LiDAR device 3 and a long-range LiDAR device 4. However, instead of this, one LiDAR device may change the ranging field of view in a time-division manner. In this way, the one LiDAR device can output the short-range point cloud P and the long-range point cloud Q to the environment point cloud generation device 6.

[0093] In addition, when performing SLAM using the long-distance point cloud Q, the application example of the above-mentioned technology for improving the accuracy of SLAM by using the close-distance point cloud P is not limited to trains 1. Application examples include all kinds of moving objects including automobiles, aircraft, and ships.

[0094] 3, the line of sight of the short-range LiDAR device 3 is aligned along the longitudinal direction of the railway 2. However, instead of this, the line of sight of the short-range LiDAR device 3 may be oblique or perpendicular to the longitudinal direction of the railway 2.

[0095] 3, the line of sight of the long-distance LiDAR device 4 is aligned along the longitudinal direction of the railway line 2. However, if the railway line 2 is curved, the line of sight of the long-distance LiDAR device 4 may be changed as appropriate.

[0096] Each drawing is merely an example for describing one or more embodiments. Each drawing may relate not only to one particular embodiment, but to one or more other embodiments. As can be understood by those skilled in the art, various features or steps described with reference to any one drawing may be combined with features or steps shown in one or more other drawings to create an embodiment not explicitly shown or described, for example. Not all features or steps shown in any one drawing are necessary to describe an exemplary embodiment, and some features or steps may be omitted. The order of steps described in any drawing may be changed as appropriate.

[0097] A part or all of the above-described embodiments can be described as, but is not limited to, the following supplementary notes. (Appendix 1) A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, Self-location estimation system. (Appendix 2) 2. A self-location estimation system according to claim 1, The precise estimation means extracts a contribution point group that contributes to the precise estimation from the far-distance point group, and precisely estimates the self-location by aligning the extracted contribution point group with the reference environment point group. Self-location estimation system. (Appendix 3) 3. The self-location estimation system according to claim 2, The contributing point group is a point group having continuity of a predetermined distance or more. Self-location estimation system. (Appendix 4) 3. The self-location estimation system according to claim 2, The vehicle further includes a structure position database storage means for storing a structure position database indicating position information of a plurality of structures that contribute to the precise estimation, the precise estimation means extracts, from the long-distance point cloud, a point cloud corresponding to at least one of the plurality of buildings as the contributing point cloud by referring to the structure position database; Self-location estimation system. (Appendix 5) 5. The self-location estimation system according to claim 4, a partial reference environment point cloud storage means for storing a partial reference environment point cloud obtained by deleting distance measurement points other than the distance measurement points corresponding to the plurality of buildings from the reference environment point cloud, The precise estimation means precisely estimates the self-location by aligning the contribution point group with the partial reference environment point group. Self-location estimation system. (Appendix 6) 2. A self-location estimation system according to claim 1, The near-distance point cloud and the far-distance point cloud are both point clouds obtained by measuring the distance ahead in the traveling direction of the vehicle. Self-location estimation system. (Appendix 7) 2. A self-location estimation system according to claim 1, the field of view of the near point cloud and the field of view of the far point cloud overlap each other; Self-location estimation system. (Appendix 8) A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, Self-location estimation device. (Appendix 9) storing a reference environment point cloud for self-location estimation; Acquire a near point cloud with a wide field of view and a far point cloud with a narrow field of view that is farther away than the near point cloud; Roughly estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; Using a result of the rough estimation as an initial condition, the far-distance point cloud is aligned with the reference environment point cloud to precisely estimate the self-location. Self-localization method. (Appendix 10) On the computer, A program for executing the self-location estimation method according to claim 9.

[0098] Some or all of the elements (e.g., configurations and functions) described in Supplementary Notes 2 to 7 that are dependent on Supplementary Note 1 may also be dependent on Supplementary Notes 8, 9, and 10 in the same dependent relationship as Supplementary Notes 2 to 7. Some or all of the elements described in any Supplementary Note may be applied to various hardware, software, recording means for recording software, systems, and methods. [Explanation of symbols]

[0099] 1 Train 2 tracks 2a sleeper 2b Rail 3 Short-range LiDAR device 3a Ranging field of view 3 Horizontal angle θ 4 Long-range LiDAR device 4a Ranging field of view 4 Horizontal angle θ 5. IMU 6 Environmental point cloud generator 7 Environmental point cloud storage device 8 Forward monitoring device 9. Drive Unit 10 Braking device 11 Multiple Wheels 11 wheels 20 point cloud acquisition section 21 Near-field point cloud storage unit 22 Far-field point cloud storage unit 23 Rough estimation part 24 Self-position memory section 25 Precise estimation section 26 Environmental point cloud generation section 30 Reference environment point cloud 31 Latest environmental point cloud 40 Obstacle detection section 41 Vehicle control unit 50 contributing points 51 Building location database storage unit 52 Building Location Database 53 Partial reference environment point cloud generator 54 Partial reference environment point cloud storage 55 Partial reference environment point cloud P Near point cloud p AF point Q far point cloud q AF point

Claims

1. A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, Self-location estimation system.

2. The self-location estimation system according to claim 1 , The precise estimation means extracts a contribution point group that contributes to the precise estimation from the far-distance point group, and precisely estimates the self-location by aligning the extracted contribution point group with the reference environment point group. Self-location estimation system.

3. The self-location estimation system according to claim 2, The contributing point group is a point group having continuity of a predetermined distance or more. Self-location estimation system.

4. The self-location estimation system according to claim 2, The vehicle further includes a structure position database storage means for storing a structure position database indicating position information of a plurality of structures that contribute to the precise estimation, the precise estimation means extracts, from the long-distance point cloud, a point cloud corresponding to at least one of the plurality of buildings as the contributing point cloud by referring to the structure position database; Self-location estimation system.

5. The self-location estimation system according to claim 4, a partial reference environment point cloud storage means for storing a partial reference environment point cloud obtained by deleting distance measurement points other than the distance measurement points corresponding to the plurality of buildings from the reference environment point cloud, The precise estimation means precisely estimates the self-location by aligning the contribution point group with the partial reference environment point group. Self-location estimation system.

6. The self-location estimation system according to claim 1 , The near-distance point cloud and the far-distance point cloud are both point clouds obtained by measuring the distance ahead in the traveling direction of the vehicle. Self-location estimation system.

7. The self-location estimation system according to claim 1 , the field of view of the near point cloud and the field of view of the far point cloud overlap each other; Self-location estimation system.

8. A reference environment point cloud storage means for storing a reference environment point cloud for self-location estimation; A point cloud acquisition means for acquiring a close-distance point cloud having a wide field of view and a long-distance point cloud having a narrow field of view and farther away than the close-distance point cloud; a coarse estimation means for coarsely estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; a precise estimation means for precisely estimating the self-location by aligning the far-distance point cloud with the reference environment point cloud using a rough estimation result by the rough estimation means as an initial condition; Including, Self-location estimation device.

9. storing a reference environment point cloud for self-location estimation; Acquire a near point cloud with a wide field of view and a far point cloud with a narrow field of view that is farther away than the near point cloud; Roughly estimating a self-location by aligning the near-distance point cloud with the reference environment point cloud; Using a result of the rough estimation as an initial condition, the far-distance point cloud is aligned with the reference environment point cloud to precisely estimate the self-location. Self-localization method.

10. On the computer, A program for executing the self-location estimation method according to claim 9.

Images