Object arrangement estimation system, object arrangement estimation device, object arrangement estimation method and program

Abstract

To estimate the arrangement of a known object on the basis of a point group having a point group defect region caused by the known object.SOLUTION: An object arrangement estimation system is provided, the system including: point group acquisition means for acquiring a point group having a point group defect region in which the point group is partially missing in a planar view due to a known object; model arrangement means for arranging an object model of the known object in a coordinate system of the point group; and adjustment means for adjusting the arrangement of the object model so that the object model, the point group defect region, and the ranging viewpoint are mutually aligned.SELECTED DRAWING: Figure 1

Description

[Technical field]

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

[0002] Patent Document 1 discloses an object recognition method for recognizing objects around a vehicle using a LiDAR device. Specifically, the method performs clustering to group measurement points output from the LiDAR device that have short inter-measurement point distances as corresponding to one object. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent Publication No. 2022-6716 Summary of the Invention [Problem to be solved by the invention]

[0004] The inventors of the present application are developing an automatic train operation technology that utilizes a LiDAR device. Specifically, the technology envisions monitoring the area ahead of the train in the direction of travel with a LiDAR device, and automatically stopping the train if an obstacle is detected.

[0005] When judging the necessity of the automatic stop, it is first necessary to judge whether the detected obstacle will interfere with the operation of the train. An obstacle on the track will interfere with the operation of the train, but an obstacle that is far away from the track will not interfere with the operation of the train.

[0006] Here, when determining whether a detected obstacle will interfere with train operation, it is possible to set an obstacle space. An obstacle space means a space in which obstacles must not exist when a train is operating. If obstacle detection is performed by limiting the point cloud output from the LiDAR device to the point cloud within the obstacle space, sufficient obstacle detection can be achieved at low calculation cost.

[0007] The above obstruction space can typically be set based on the rail arrangement identified by the point cloud output from the LiDAR device. The rail arrangement means the position and orientation of the rail in three-dimensional space. However, the upper surface of the rail is generally a glossy mirror surface. Therefore, it is inevitable that the point cloud corresponding to the upper surface of the rail is missing from the point cloud output from the LiDAR device. Therefore, it is difficult to identify the rail arrangement based on the point cloud output from the LiDAR device, and as a result, it is not possible to set an obstruction space ahead of the train in the traveling direction.

[0008] Therefore, an object of the present disclosure is to provide a technique for estimating the arrangement of a known object based on a point cloud that has a point cloud missing area caused by the known object. [Means for solving the problem]

[0009] a point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, An object position estimation system is provided.

[0010] a point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, An object position estimation device is provided.

[0011] A point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object is obtained; placing an object model of the known object in the coordinate system of the point cloud; adjusting a position of the object model so that the object model, the point cloud missing region, and the distance measurement viewpoint are aligned with each other; A method for estimating object position is provided. Effect of the Invention

[0012] According to the present disclosure, the arrangement of a known object can be estimated based on a point cloud that has a point cloud missing area caused by the known object. [Brief description of the drawings]

[0013] [Figure 1] FIG. 1 is a block diagram of an object position estimation system. [Diagram 2] FIG. 2 is a block diagram of a train. [Diagram 3] FIG. 2 is a diagram showing the latest driving environment point cloud. [Figure 4] 1 is a diagram showing an object model placed in the coordinate system of the latest driving environment point cloud. [Diagram 5] 11A and 11B are diagrams illustrating a method for extracting a specific distance measurement point. [Figure 6] 11A and 11B are diagrams for explaining a method of calculating the amount of deviation of a specific distance measurement point. [Figure 7] 11A and 11B are diagrams for explaining a method of calculating the amount of deviation of a specific distance measurement point. [Figure 8] 11A and 11B are diagrams for explaining a method of calculating the amount of deviation of a specific distance measurement point. [Figure 9] 13 is a diagram showing a specific example of an abnormality warning screen. [Figure 10] 4 is a control flow of the forward monitoring device. [Figure 11]4 is a control flow of the forward monitoring device. [Figure 12] 11A and 11B are diagrams for explaining a method of calculating the amount of deviation of a specific distance measurement point. [Figure 13] 11A and 11B are diagrams illustrating a method for extracting a specific distance measurement point. [Figure 14] 4 is a control flow of the forward monitoring device. [Figure 15] 4 is a control flow of the forward monitoring device. [Figure 16] FIG. 2 is a diagram showing a case where a processing circuit provided in a forward monitoring device is configured with a processor and a memory. [Figure 17] FIG. 13 is a diagram showing a case where a processing circuit provided in the forward monitoring device is configured with dedicated hardware. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0014] (Summary of the Disclosure) An outline of the present disclosure will be described below. Fig. 1 is a block diagram of an object position estimation system 100. As shown in Fig. 1, the object position estimation system 100 includes a point cloud acquisition unit 101, a model placement unit 102, and an adjustment unit 103.

[0015] The point cloud acquisition means 101 acquires a point cloud having a point cloud missing region in which the point cloud is partially missing in plan view due to a known object.

[0016] The model placement means 102 places an object model of a known object in the coordinate system of the point cloud.

[0017] The adjustment means 103 adjusts the arrangement of the object model so that the object model, the point cloud missing area, and the distance measurement viewpoint are consistent with each other.

[0018] According to the above configuration, the arrangement of a known object can be estimated based on a point cloud that has a point cloud missing area caused by the known object.

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

[0020] 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.

[0021] The train 1 includes a point cloud generating device 3, a point cloud storage device 4, a forward monitoring device 5, a driving device 6, a braking device 7, and an output device 8.

[0022] The drive device 6 includes a motor and a motor driver. The drive device 6 drives the wheels 9 in accordance with a control signal from the forward monitoring device 5.

[0023] The braking device 7 is composed of a brake and a hydraulic circuit. The braking device 7 brakes the wheels 9 in accordance with a control signal from the forward monitoring device 5.

[0024] The output device 8 is typically configured with a display capable of screen output and a speaker capable of audio output.

[0025] The point cloud generating device 3 generates a point cloud of the traveling environment ahead in the traveling direction of the track 2. Specifically, the point cloud generating device 3 includes a LiDAR device 20, an IMU (Inertial Measurement Unit) 21, a GNSS (Global Navigation Satellite System) 22, a movement compensation unit 23, a partial point cloud storage unit 24, and a SLAM execution unit 25 (Simultaneous Localization and Mapping).

[0026] The LiDAR device 20 is a specific example of a sensing means for sensing the front of the train 1 in the traveling direction to generate a three-dimensional point cloud. The LiDAR device 20 of this embodiment is a direct ToF (Time of Flight) type. That is, the LiDAR device 20 generates a three-dimensional point cloud in the traveling direction of the train 1 in the forward direction by emitting a laser light in the traveling direction of the train 1 and measuring the time required to receive the reflected light. However, instead of this, the LiDAR device 20 may be an FMCW (Frequency Modulated Continuous Wave) type that generates the above-mentioned three-dimensional point cloud based on the frequency difference between the laser light emitted in the traveling direction of the train 1 and the reflected light. In addition, the LiDAR device 20 may be an indirect ToF type that generates the above-mentioned three-dimensional point cloud based on the phase difference between the laser light emitted in the traveling direction of the train 1 and the reflected light. The LiDAR device 20 accumulates the generated three-dimensional point cloud (hereinafter also simply referred to as a point cloud) in a partial point cloud storage unit 24 as a partial point cloud.

[0027] The sensing means is not limited to the LiDAR device 20. Any device capable of sensing the area ahead of the train 1 in the traveling direction may be used as the sensing means. For example, the sensing means may be configured to generate a three-dimensional 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 three-dimensional 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.

[0028] The IMU 21 detects the three-axis acceleration and three-axis angular velocity of the train 1 .

[0029] The GNSS 22 detects the position of the train 1 .

[0030] The motion compensation unit 23 performs motion compensation on the partial point clouds stored in the partial point cloud storage unit 24 based on the detection results of the IMU 21 .

[0031] The SLAM execution unit 25 executes SLAM processing consisting of self-location estimation of the train 1 and point cloud reconstruction of the running environment of the train 1. Here, the point cloud storage device 4 stores the previous running environment point cloud 4a and the latest running environment point cloud 4b. The previous running environment point cloud 4a and the latest running environment point cloud 4b are both point clouds corresponding to the running environment extending along the track 2. The SLAM execution unit 25 then estimates the self-location of the train 1 corresponding to the partial point cloud by aligning the partial point cloud of the partial point cloud storage unit 24 with the previous running environment point cloud 4a created in the past. At this time, the SLAM execution unit 25 sets the initial position of the partial point cloud based on the detection result of the GNSS 22 as an initial condition at the time of alignment. The SLAM execution unit 25 also generates the latest running environment point cloud 4b by overlapping the partial point clouds multiple times based on the self-location estimation result. In short, the SLAM execution unit 25 generates the latest driving environment point cloud 4b based on the partial point cloud stored in the partial point cloud storage unit 24 and the previous driving environment point cloud 4a. The SLAM execution unit 25 typically executes the above SLAM process every 100 milliseconds.

[0032] The forward monitoring device 5 monitors the forward traveling direction of the train 1 based on the latest traveling environment point cloud 4b. Typically, the forward monitoring device 5 monitors whether there is an obstacle ahead in the traveling direction of the train 1 or whether there is an abnormality in the track 2 based on the latest traveling environment point cloud 4b.

[0033] The forward monitoring device 5 includes a point cloud acquisition unit 30, a model placement unit 31, an adjustment unit 32, an obstruction space setting unit 33, an obstacle determination unit 34, and an abnormality determination unit 35.

[0034] The point cloud acquisition unit 30 acquires the latest driving environment point cloud 4b from the point cloud storage device 4. FIG. 3 shows a specific example of the latest driving environment point cloud 4b. In FIG. 3, the area where a large number of point clouds are distributed is shown by hatching. As shown in FIG. 3, the latest driving environment point cloud 4b has a point cloud missing area 40 where a point cloud is missing in a planar view due to the rail 2b. The point cloud missing area 40 is distributed in an area that is a blind spot of the rail 2b as viewed from the LiDAR device 20. The rail 2b is a specific example of a known object. That is, the cross-sectional shape and cross-sectional dimensions of the rail 2b are known.

[0035] The model placement unit 31 places the object model 41 of the rail 2b in the coordinate system 4c of the latest driving environment point cloud 4b. FIG. 4 shows the object model 41 placed in the coordinate system 4c of the latest driving environment point cloud 4b. The object model 41 is three-dimensional shape data of the rail 2b created in advance from a three-dimensional design drawing of the rail 2b or the like. The model placement unit 31 places the object model 41 in the coordinate system 4c so that the object model 41 overlaps with the point cloud missing area 40 in a planar view. In addition, the model placement unit 31 detects multiple sleepers 2a based on the latest driving environment point cloud 4b, and places the object model 41 in the coordinate system 4c so that it contacts the multiple sleepers 2a in the vertical direction. In short, the model placement unit 31 provisionally places the object model 41 in the coordinate system 4c of the latest driving environment point cloud 4b. Similarly, the model placement unit 31 sets a distance measurement viewpoint 42. The ranging viewpoint 42 indicates the position in the coordinate system 4c of the LiDAR device 20 at the time of measuring the latest driving environment point cloud 4b. The model placement unit 31 typically sets the ranging viewpoint 42 based on the self-position estimation result by the SLAM execution unit 25 of the point cloud generation device 3.

[0036] The adjustment unit 32 adjusts the arrangement of the object model 41 so that the object model 41, the point cloud missing region 40, and the distance measurement viewpoint 42 are consistent with each other.

[0037] 4, area 43, which is an area of ​​point cloud missing area 40 that is closer to ranging viewpoint 42 than object model 41 in plan view, is an area that is not in the blind spot of object model 41, and therefore a sufficient point cloud should have been obtained without belonging to point cloud missing area 40. Therefore, with regard to area 43, it can be said that object model 41, point cloud missing area 40, and ranging viewpoint 42 are not consistent with each other.

[0038] Areas 44 and 45 of the latest driving environment point cloud 4b, which are areas farther from the ranging viewpoint 42 than the object model 41 in a planar view, are areas in the blind spot of the object model 41, and therefore should essentially belong to the point cloud missing area 40, and a point cloud should not have been acquired from them. Therefore, with regard to areas 44 and 45, it can be said that the object model 41, the point cloud missing area 40, and the ranging viewpoint 42 are not consistent with each other.

[0039] In this way, when the object model 41, the point cloud missing area 40, and the ranging viewpoint 42 are not consistent with each other, the adjustment unit 32 adjusts the arrangement of the object model 41 to achieve the above-mentioned consistency. Here, the arrangement of the object model 41 refers to the position and orientation of the object model 41 in the coordinate system 4c. This allows the adjustment unit 32 to estimate the arrangement of the rail 2b even when the latest driving environment point cloud 4b does not include any point cloud corresponding to the rail 2b. Here, the arrangement of the rail 2b refers to the position and orientation of the rail 2b in the coordinate system 4c, similar to the object model 41.

[0040] When attempting to align the object model 41, the point cloud missing area 40, and the ranging viewpoint 42, the adjustment unit 32 may check the alignment for all ranging points x included in the latest driving environment point group 4b. However, since the number of ranging points x included in the latest driving environment point group 4b generally reaches tens of thousands, the processing time required to achieve alignment is not realistic. Therefore, the adjustment unit 32 extracts multiple specific ranging points X from the multiple ranging points x included in the latest driving environment point group 4b. Then, the adjustment unit 32 attempts to align the object model 41, the multiple specific ranging points X, and the ranging viewpoint 42.

[0041] Please refer to FIG. 5. FIG. 5 is a diagram showing a plurality of specific measurement points X. In this embodiment, it is assumed that the spatial resolution of the latest driving environment point cloud 4b is sufficiently high. Therefore, the contour 46 of the point cloud missing region 40 can be specified typically by using a convex hull algorithm. Therefore, in this embodiment, as the plurality of specific measurement points X, the measurement points x corresponding to the contour 46 of the point cloud missing region 40 are adopted.

[0042] Next, the adjustment unit 32 calculates the amount of deviation δ of a plurality of line of sight segments 47 passing through each of the plurality of specific distance measurement points X and the distance measurement viewpoint 42 from the outer surface 41a of the object model 41. In short, the adjustment unit 32 calculates the amount of deviation δ of the plurality of specific distance measurement points X. The adjustment unit 32 calculates the amount of deviation δ of each specific distance measurement point X.

[0043] Fig. 6 to Fig. 8 are diagrams for explaining a method for calculating the deviation amount δ. Fig. 6 and Fig. 7 show cross-sectional views of the object model 41 when the line of sight segment 47 penetrates the object model 41. Fig. 8 shows a cross-sectional view of the object model 41 when the line of sight segment 47 does not penetrate the object model 41.

[0044] As shown in FIG. 6, the adjustment unit 32 generates a line of sight 47 passing through a specific distance measurement point X and the distance measurement viewpoint 42. When the line of sight 47 penetrates the object model 41, the adjustment unit 32 first defines a plurality of search points p on an internal line portion 47a of the line of sight 47 that extends inside the outer surface 41a of the object model 41. In the example of FIG. 6, the adjustment unit 32 defines a search point p(1) to a search point p(17) on the internal line portion 47a of the line of sight 47. Next, the adjustment unit 32 calculates the shortest distance d to the outer surface 41a of the object model 41 for each search point p. Specifically, the adjustment unit 32 generates a plurality of search line segments s that extend radially from the search point p toward the outer surface 41a of the object model 41 for each search point p and reach the outer surface 41a of the object model 41. In FIG. 6, the plurality of search line segments s corresponding to the search point p(11) are indicated by dashed lines. Next, the adjustment unit 32 specifies the shortest search line segment s among the multiple search line segments s as the shortest search line segment t. Then, the adjustment unit 32 sets the shortest distance d to the outer surface 41a of the object model 41 for each search point p as the distance of the shortest search line segment t. That is, the shortest distance d to the outer surface 41a of the object model 41 for each search point p is the length of the shortest search line segment t of the search point p. Next, as shown in FIG. 7, the adjustment unit 32 calculates the shortest distance d of each remaining search point p. Then, the adjustment unit 32 sets the shortest distance d that is the maximum among the shortest distances d of the multiple search points p as the deviation amount δ of the specific ranging point X corresponding to the line of sight segment 47. In the example of FIG. 7, since the shortest distance d of the search point p (13) is the longest compared to the shortest distances d of the other search points p, the deviation amount δ of the specific ranging point X corresponding to the line of sight segment 47 is set to the shortest distance d of the search point p (13).

[0045] As shown in Figure 8, when the line of sight segment 47 does not penetrate the object model 41, the deviation amount δ of the specific distance measurement point X corresponding to the line of sight segment 47 is set to the shortest distance e between the line of sight segment 47 and the outer surface 41a of the object model 41.

[0046] In this way, the adjustment unit 32 calculates the deviation amount δ for each specific distance measurement point X. Next, the adjustment unit 32 calculates the sum F(r) of the deviation amounts δ for the multiple specific distance measurement points X based on the following formula (1). Note that in the following formula (1), the variable r of the deviation amount δ(X, r) represents the arrangement of the object model 41.

[0047]

number

[0048] Next, the adjustment unit 32 searches for a layout r* of the object model 41 that minimizes the sum F(r) calculated by the above formula (1), as shown in the following formula (2).

[0049]

number

[0050] The above-mentioned arrangement r* of the object model 41 means the arrangement of the object model 41 that best matches the object model 41, the point cloud missing region 40, and the distance measurement viewpoint 42. In this way, the adjustment unit 32 estimates the arrangement of the rail 2b with high accuracy.

[0051] The obstruction space setting unit 33 sets an obstruction space corresponding to the track 2 based on the arrangement of the rails 2b estimated by the adjustment unit 32. As described above, the obstruction space means a space in which no obstacles should exist when the train 1 is operated. Typically, the obstruction space corresponds to the swept volume of the train 1. The swept volume of the train 1 can be uniquely calculated based on the arrangement of the rails 2b and the dimensions of the train 1.

[0052] The obstacle determination unit 34 determines whether or not there is an obstacle in the obstructed space set by the obstructed space setting unit 33. Specifically, the obstacle determination unit 34 determines whether or not there is an obstacle in the obstructed space by executing an object detection process such as PointNet on the point cloud in the obstructed space. Alternatively, the obstacle determination unit 34 may determine whether or not there is an obstacle in the obstructed space by simply comparing the previous traveling environment point cloud 4a with the latest traveling environment point cloud 4b in the obstructed space. When the obstacle determination unit 34 detects an obstacle in the obstructed space, it outputs a stop signal to the braking device 7. As a result, the obstacle determination unit 34 stops the train 1, and a collision between the train 1 and the obstacle can be avoided.

[0053] The abnormality determination unit 35 detects a local abnormality of the rail 2b based on the deviation amount δ of the multiple specific distance measurement points X. The local abnormality of the rail 2b is typically a distortion of the rail 2b itself or a foreign object placed on the rail 2b. For example, the abnormality determination unit 35 identifies the largest deviation amount δ among the deviation amounts δ of the multiple specific distance measurement points X after the adjustment unit 32 estimates the arrangement of the rail 2b. The abnormality determination unit 35 determines whether the deviation amount δ exceeds a threshold value. If the deviation amount δ exceeds the threshold value, the abnormality determination unit 35 determines that the rail 2b has a local abnormality at one of two intersections 41b where the line of sight segment 47 corresponding to the deviation amount δ intersects with the outer surface 41a of the object model 41, as shown in FIG. 9. If the abnormality determination unit 35 determines that the rail 2b has a local abnormality, it outputs an abnormality warning image as shown in FIG. 9 to the output device 8. The abnormality warning screen typically contains content that prompts the driver to inspect the abnormal part of the rail 2b. This allows the driver of the train 1 to stop the train 1 based on the abnormality warning image output to the output device 8, and to prevent the train 1 from passing through the abnormal part of the rail 2b.

[0054] Next, there will be described a control flow of the forward monitoring device 5. FIG 10 and FIG 11 show a control flow of the forward monitoring device 5.

[0055] As shown in FIG. 10, first, the point cloud acquisition unit 30 acquires the latest driving environment point cloud 4b from the point cloud storage device 4 (S100). Next, the model placement unit 31 sets the ranging viewpoint 42 (S110). Next, the model placement unit 31 sets the placement of the object model 41 (S120). Next, the adjustment unit 32 extracts a plurality of specific ranging points X from the plurality of ranging points x included in the latest driving environment point cloud 4b (S130). Next, the adjustment unit 32 calculates deviation amounts δ of the plurality of specific ranging points X (S140). Next, the adjustment unit 32 calculates the sum of deviation amounts δ of the plurality of specific ranging points X (S150).

[0056] Next, the adjustment unit 32 changes the arrangement of the object model 41 (S160).

[0057] Next, the adjustment unit 32 calculates the deviation amount δ of the multiple specific distance measurement points X (S180). Next, the adjustment unit 32 calculates the sum of the deviation amounts δ of the multiple specific distance measurement points X (S190).

[0058] Next, the adjustment unit 32 determines whether the difference between the sum calculated last time and the sum calculated this time is below a threshold value (S200). In other words, the adjustment unit 32 determines whether the sum of the deviation amounts δ of the multiple specific distance measurement points X has converged. If the difference is not below the threshold value (S200: NO), the adjustment unit 32 returns the process to step S160. On the other hand, if the difference is below the threshold value (S200: YES), the adjustment unit 32 determines that the sum of the deviation amounts δ of the multiple specific distance measurement points X has converged, and proceeds to step S210. In step S210, the adjustment unit 32 estimates the arrangement of the rail 2b (S210).

[0059] As shown in FIG. 11, next, the obstruction space setting unit 33 sets an obstruction space (S220). Next, the obstacle determination unit 34 determines whether or not there is an obstacle in the obstruction space set in step S220 (S230). If an obstacle is detected in step S230 (S230: YES), the obstacle determination unit 34 executes collision avoidance control by outputting a stop signal to the braking device 7 (S240), and the process proceeds to step S250. If an obstacle is not detected in step S230 (S230: NO), the obstacle determination unit 34 proceeds to step S250. In step S250, the abnormality determination unit 35 detects a local abnormality of the rail 2b based on the deviation amount δ of the multiple specific distance measurement points X (S250). If the abnormality determination unit 35 detects a local abnormality of the rail 2b (S250: YES), it outputs an abnormality warning screen to the output device 8 (S260), and ends the process. On the other hand, if the abnormality determination unit 35 has not detected a local abnormality in the rail 2b (S250: NO), the process ends.

[0060] Here, the above step S160 will be explained in more detail. In the above step S160, the adjustment unit 32 changes the arrangement of the object model 41. At this time, the adjustment unit 32 changes the arrangement of the object model 41 (S160) so that the difference finally falls below the threshold (S200: YES). For this reason, the adjustment unit 32 typically changes the arrangement of the object model 41 so that the arrangement of the object model 41 converges using the Newton method or the Levenberg-Marquardt method. When the difference falls below the threshold in step S200, the arrangement of the object model 41 reaches a tentative local solution. In this way, the arrangement of the object model 41 reaching a tentative local solution is nothing but calculating the minimum value of the sum F(r) in the above formula (2). However, instead of determining that the difference falls below the threshold (S200), it may be more simply determined that the sum F(r) falls below the threshold.

[0061] The first embodiment of the present disclosure has been described above. This embodiment has the following features.

[0062] That is, in the first embodiment, the object position estimation system is realized by a point cloud acquisition unit 30, a model placement unit 31, and an adjustment unit 32. The point cloud acquisition unit 30 acquires a latest traveling environment point cloud 4b (point cloud) having a point cloud missing area 40 in which the point cloud is partially missing in a planar view due to a rail 2b (known object). The model placement unit 31 places an object model 41 of the rail 2b in a coordinate system 4c of the latest traveling environment point cloud 4b. The adjustment unit 32 adjusts the placement of the object model 41 so that the object model 41, the point cloud missing area 40, and the ranging viewpoint 42 are consistent with each other. With the above configuration, it is possible to estimate the position of a known object based on a point cloud having a point cloud missing area due to a known object.

[0063] 5, the object model 41, the point cloud missing region 40, and the ranging viewpoint 42 being consistent with each other means that the object model 41, the multiple specific ranging points X, and the ranging viewpoint 42 are consistent with each other. The multiple specific ranging points X are ranging points x that correspond to the contour 46 of the point cloud missing region 40 among the multiple ranging points x that constitute the latest traveling environment point cloud 4b. With the above configuration, the processing time of the adjustment unit 32 can be shortened. Therefore, obstacle judgment and abnormality judgment can be performed in real time while the train 1 is traveling.

[0064] Moreover, the object model 41, the multiple specific distance measurement points X, and the distance measurement viewpoint 42 are aligned with each other when the sum F(r) of the deviation amounts δ from the outer surface 41a of the object model 41 of the multiple line of sight segments 47 passing through each of the multiple specific distance measurement points X and the distance measurement viewpoint 42 satisfies a predetermined condition. According to the above configuration, the object model 41, the multiple specific distance measurement points X, and the distance measurement viewpoint 42 can be aligned with each other based on the index of the deviation amount δ.

[0065] The object position estimation system further includes an abnormality determination unit 35 (abnormality detection means). The abnormality determination unit 35 detects a local abnormality of the rail 2b based on the deviation amount δ of the multiple specific distance measurement points X. According to the above configuration, it is possible to detect a local abnormality of the rail 2b by using the deviation amount δ of the multiple specific distance measurement points X calculated when detecting an obstacle, without requiring additional complex calculations.

[0066] Second embodiment Next, a second embodiment of the present disclosure will be described. Below, the differences between the second embodiment and the first embodiment will be mainly described, and overlapping descriptions will be omitted. Fig. 12 is a diagram for explaining a method of calculating the deviation amount δ of a plurality of specific distance measuring points X.

[0067] In the first embodiment, as shown in FIGS. 6 to 8, the adjustment unit 32 generates a plurality of line-of-sight segments 47 passing through each of the plurality of specific distance measurement points X and the distance measurement viewpoint 42, and calculates the deviation amount δ for each line-of-sight segment 47.

[0068] 12, the adjustment unit 32 projects a plurality of specific ranging points X onto the perspective projection plane 50 of the ranging viewpoint 42. The adjustment unit 32 also projects an object model 41 onto the perspective projection plane 50 of the ranging viewpoint 42. The adjustment unit 32 then calculates, for each specific ranging point X, the shortest distance u from the specific ranging point X to the contour 51 of the object model 41 on the perspective projection plane 50 as the deviation amount δ. In this way, the adjustment unit 32 calculates the deviation amounts δ of the multiple specific ranging points X. The adjustment unit 32 calculates the deviation amounts δ of each specific ranging point X.

[0069] When the object model 41 is expressed by a point cloud, the contour 51 of the object model 41 can typically be specified by using a convex hull algorithm. On the other hand, when the object model 41 is expressed by three-dimensional shape data, the contour 51 of the object model 41 can be more easily specified by simple geometric calculations.

[0070] The second embodiment of the present disclosure has been described above. This embodiment has the following features.

[0071] That is, the object model 41, the multiple specific distance measurement points X, and the distance measurement viewpoint 42 are consistent with each other when the sum F(r) of the deviation amounts δ of each of the multiple specific distance measurement points X projected onto the perspective projection plane 50 of the distance measurement viewpoint 42 from the contour 51 of the object model 41 projected onto the perspective projection plane 50 satisfies a predetermined condition. With the above configuration, the processing time of the adjustment unit 32 can be shortened. Therefore, obstacle judgment and abnormality judgment can be performed in real time while the train 1 is traveling.

[0072] Third embodiment Next, a third embodiment of the present disclosure will be described. Below, the differences between the third embodiment and the first embodiment will be mainly described, and overlapping descriptions will be omitted. Fig. 13 is a diagram for explaining a method of extracting a specific distance measuring point X.

[0073] In the above first embodiment, as shown in FIG. 5, the distance measurement points x corresponding to the contour 46 of the point cloud missing region 40 are adopted as the multiple specific distance measurement points X.

[0074] In contrast, in this embodiment, as shown in Fig. 13, among the multiple distance measurement points x constituting the latest driving environment point group 4b, distance measurement points x whose line of sight segments 47 (lines) connecting the corresponding distance measurement points x and the distance measurement viewpoint 42 penetrate the object model 41 are adopted as the multiple specific distance measurement points X. In the example of Fig. 13, among the multiple distance measurement points x constituting the latest driving environment point group 4b, distance measurement points x belonging to areas 60 and 61 have the property that the line of sight segments 47 connecting the corresponding distance measurement points x and the distance measurement viewpoint 42 penetrate the object model 41, and therefore are adopted as the above-mentioned specific distance measurement points X.

[0075] In this way, in order to shorten the processing time of the adjustment unit 32, it is effective to narrow down the number of ranging points x when checking the above-mentioned consistency. In short, it would be preferable to adopt the ranging points x in the vicinity of the point cloud missing region 40 as the specific ranging points X.

[0076] 14 shows a control flow of the forward monitoring device 5. As shown in FIG. 14, in this embodiment, step S170 in which the adjustment unit 32 extracts a plurality of specific distance measurement points X is added between step S160 and step S180. This is because, every time the arrangement of the object model 41 is changed (S160), among the plurality of distance measurement points x constituting the latest driving environment point group 4b, a distance measurement point x whose line of sight segment 47 connecting the distance measurement point x and the distance measurement viewpoint 42 penetrates the object model 41 is replaced.

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

[0078] That is, the object model 41, the point cloud missing region 40, and the ranging viewpoint 42 being consistent with each other means that the object model 41, a plurality of specific ranging points X as ranging points x among the plurality of ranging points x constituting the latest traveling environment point cloud 4b, where a line of sight segment 47 connecting the ranging point x and the ranging viewpoint 42 passes through the object model 41, and the ranging viewpoint 42 are consistent with each other. According to the above configuration, the processing time of the adjustment unit 32 can be shortened. Therefore, obstacle judgment and abnormality judgment can be performed in real time while the train 1 is traveling.

[0079] (Fourth embodiment) Next, a fourth embodiment of the present disclosure will be described. Hereinafter, the differences between the fourth embodiment and the first embodiment will be mainly described, and overlapping descriptions will be omitted. Fig. 15 shows a control flow of the forward monitoring device.

[0080] In this embodiment, the control flow of the forward monitoring device 5 is different from that of the first embodiment. That is, in this embodiment, between steps S160 and S180, step S165 in which the adjustment unit 32 of the forward monitoring device 5 changes the ranging viewpoint 42 is added. That is, in this embodiment, the adjustment unit 32 adjusts the arrangement of the object model 41, and also adjusts the ranging viewpoint 42.

[0081] For example, the model placement unit 31 sets the ranging viewpoint 42 based on the self-location estimation result by the SLAM execution unit 25 of the point cloud generation device 3 (S110). Alternatively, the model placement unit 31 may set the ranging viewpoint 42 based on the self-location detection result by the GNSS 22, or may set the ranging viewpoint 42 based on the self-location estimation result using another self-location estimation algorithm such as Visual SLAM. Here, the self-location detection result by the GNSS 22 and the self-location estimation result using another self-location estimation algorithm such as Visual SLAM have poor position accuracy compared to the LiDAR SLAM of this embodiment. That is, the setting accuracy when the model placement unit 31 sets the ranging viewpoint 42 is poor. Therefore, in this embodiment, the ranging viewpoint 42 is also actively adjusted as a parameter of the convergence calculation in step S200.

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

[0083] The adjustment unit 32 adjusts the arrangement of the object model 41 and the ranging viewpoint 42 so that the object model 41, the point cloud missing region 40, and the ranging viewpoint 42 are consistent with each other. According to the above configuration, the arrangement of the object model 41 can be realized with higher accuracy compared to the embodiment in which the ranging viewpoint 42 set with low accuracy is continuously used as a fixed value.

[0084] Next, a description will be given of the hardware configuration of the forward monitoring device 5. In the forward monitoring device 5, the point cloud acquisition unit 30, the model placement unit 31, the adjustment unit 32, the obstruction space setting unit 33, the obstacle determination unit 34, and the abnormality determination unit 35 are realized by processing circuits. The processing circuit may be a processor and memory that executes a program stored in a memory, or may be dedicated hardware.

[0085] FIG. 16 is a diagram showing an example in which the processing circuit of the forward monitoring device 5 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 forward monitoring device 5 is realized by software, firmware, or a combination of software and firmware. The software or firmware is described 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 a memory 1001 for storing a program that results in the processing of the forward monitoring device 5 being executed. It can also be said that these programs cause a computer to execute the procedures and methods of the forward monitoring device 5.

[0086] 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.

[0087] Fig. 17 is a diagram showing an example in which the processing circuit of the forward monitoring device 5 is configured with dedicated hardware. When the processing circuit is configured with dedicated hardware, the processing circuit 1002 shown in Fig. 17 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 forward monitoring device 5 may be realized by the processing circuit 1002 on a function-by-function basis, or each function may be realized by the processing circuit 1002 collectively.

[0088] It should be noted that the functions of the forward monitoring device 5 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.

[0089] 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.

[0090] 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.

[0091] The above first to fourth embodiments can be implemented in appropriate combinations.

[0092] 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 point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, Object placement estimation system. (Appendix 2) 2. The object position estimation system according to claim 1, The object model, the point cloud missing region, and the ranging viewpoint being consistent with each other is the object model, a plurality of specific distance measurement points among a plurality of distance measurement points constituting the point cloud, which are distance measurement points whose line segments connecting the distance measurement points and the distance measurement viewpoint pass through the object model, and the distance measurement viewpoint are consistent with each other; Object placement estimation system. (Appendix 3) 2. The object position estimation system according to claim 1, The object model, the point cloud missing region, and the ranging viewpoint being consistent with each other is the object model, a plurality of specific ranging points among a plurality of ranging points constituting the point cloud, which are ranging points corresponding to the contour of the point cloud missing area, and the ranging viewpoint are consistent with each other. Object placement estimation system. (Appendix 4) 4. The object position estimation system according to claim 2, The object model, the plurality of specific distance measurement points, and the distance measurement viewpoint being consistent with each other is a sum of deviations of a plurality of line-of-sight segments passing through each of the plurality of specific distance measurement points and the distance measurement viewpoint from an outer surface of the object model satisfies a predetermined condition; Object placement estimation system. (Appendix 5) 4. The object position estimation system according to claim 2, The object model, the plurality of specific distance measurement points, and the distance measurement viewpoint being consistent with each other is a sum of deviations of each of the specific distance measurement points projected on a perspective projection plane of the distance measurement viewpoint from a contour of the object model projected on the perspective projection plane satisfies a predetermined condition; Object placement estimation system. (Appendix 6) 2. The object position estimation system according to claim 1, the adjustment means adjusts a position of the object model and the ranging viewpoint so that the object model, the point cloud missing region, and the ranging viewpoint are consistent with each other. Object placement estimation system. (Appendix 7) 5. The object position estimation system according to claim 4, an anomaly detection unit that detects a local anomaly of the known object based on the amount of deviation of the plurality of specific distance measurement points; Object placement estimation system. (Appendix 8) a point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, Object placement estimation device. (Appendix 9) A point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object is obtained; placing an object model of the known object in the coordinate system of the point cloud; adjusting a position of the object model so that the object model, the point cloud missing region, and the distance measurement viewpoint are aligned with each other; Object placement estimation method. (Appendix 10) A program for causing a computer to execute the object position estimation method according to claim 9.

[0093] 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]

[0094] 1 Train 2 tracks 2a Sleepers 2b Rail 3 Point cloud generator 4 point cloud storage 4a Previous driving environment point cloud 4b Latest driving environment point cloud 4c Coordinate System 5 Forward monitoring device 6. Drive unit 7 Braking device 8 Output Devices 9 wheels 20 LiDAR device 21 IMU 22 GNSS 23 Movement Compensation Department 24 Partial point cloud storage 25 SLAM Executive Team 30 point cloud acquisition section 31 Model placement section 32 Adjustment part 33 Obstruction Space Setting Section 34 Obstacle detection section 35 Abnormality determination section 40 Point cloud missing area 41 Object Model 41a Outer surface 42 Distance measurement viewpoints 43 Region 44 Region 45 Region 46 Contour 47 Line segment of line of sight 47a Inner line part 50 Perspective projection plane 51 Contour 60 Region 61 Region x Distance measurement point X Specific distance measurement point δ Deviation amount d Shortest distance e Shortest distance u Shortest distance p Exploration point s Exploration line segment t Shortest exploration line segment

Claims

1. a point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, Object placement estimation system.

2. The object position estimation system according to claim 1 , The object model, the point cloud missing region, and the ranging viewpoint being consistent with each other is the object model, a plurality of specific distance measurement points among a plurality of distance measurement points constituting the point cloud, which are distance measurement points whose line segments connecting the distance measurement points and the distance measurement viewpoint pass through the object model, and the distance measurement viewpoint are consistent with each other; Object placement estimation system.

3. The object position estimation system according to claim 1 , The object model, the point cloud missing region, and the ranging viewpoint being consistent with each other is the object model, a plurality of specific ranging points among a plurality of ranging points constituting the point cloud, which are ranging points corresponding to the contour of the point cloud missing area, and the ranging viewpoint are consistent with each other. Object placement estimation system.

4. The object position estimation system according to claim 2 or 3, The object model, the plurality of specific distance measurement points, and the distance measurement viewpoint being consistent with each other is a sum of deviations of a plurality of line-of-sight segments passing through each of the plurality of specific distance measurement points and the distance measurement viewpoint from an outer surface of the object model satisfies a predetermined condition; Object placement estimation system.

5. The object position estimation system according to claim 2 or 3, The object model, the plurality of specific distance measurement points, and the distance measurement viewpoint being consistent with each other is a sum of deviations of each of the specific distance measurement points projected on a perspective projection plane of the distance measurement viewpoint from a contour of the object model projected on the perspective projection plane satisfies a predetermined condition; Object placement estimation system.

6. The object position estimation system according to claim 1 , the adjustment means adjusts a position of the object model and the ranging viewpoint so that the object model, the point cloud missing region, and the ranging viewpoint are consistent with each other. Object placement estimation system.

7. The object position estimation system according to claim 4, an anomaly detection unit that detects a local anomaly of the known object based on the amount of deviation of the plurality of specific distance measurement points; Object placement estimation system.

8. a point cloud acquisition means for acquiring a point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object; a model arrangement means for arranging an object model of the known object in the coordinate system of the point cloud; an adjustment means for adjusting a position of the object model so that the object model, the point cloud missing area, and a distance measurement viewpoint are aligned with each other; Including, Object placement estimation device.

9. A point cloud having a point cloud missing region in which the point cloud is partially missing in a planar view due to a known object is obtained; placing an object model of the known object in the coordinate system of the point cloud; adjusting a position of the object model so that the object model, the point cloud missing region, and the distance measurement viewpoint are aligned with each other; Object placement estimation method.

10. A program for causing a computer to execute the object position estimation method according to claim 9.

Images