Direction angle detection device

Abstract

This technology enables stable detection of directional angles in heavy machinery and equipment that autonomously travel within tunnels, without being affected by obstacles. [Solution] The arch shape detection unit 101 measures the distance between the center point of the LiDAR scan and the side or top for each of several scanning angles, and detects the arch shape of the tunnel based on the set of distance values ​​acquired for each scanning angle. The direction angle calculation unit 102 stores in advance the set of distance values ​​detected by the arch shape detection unit when the heavy machinery / device is pointed towards each of several reference direction angles, as reference arch shapes corresponding to those reference direction angles. When the heavy machinery / device 100 is autonomously driving, the arch shape detection unit calculates the direction angle corresponding to the reference direction angle that is closest to the arch shape detected by the arch shape detection unit based on the set of detected distance values ​​among the multiple reference arch shapes stored in advance.

Description

[Technical Field]

[0001] This invention relates to a technology for controlling the directional angle of heavy machinery and equipment that autonomously travels inside tunnels. [Background technology]

[0002] For example, in a mountain tunnel construction site, when various heavy machinery and equipment are autonomously driven inside the tunnel using control technologies such as SLAM (Simultaneous Localization and Mapping), it is crucial to recognize the angle of the direction (direction of travel) of the heavy machinery and equipment relative to the tunnel excavation direction. While the face direction and tunnel entrance direction can be determined from the situation at the time of use, using directional angle information allows for more detailed control.

[0003] Conventionally, a technology has been proposed to determine the direction of heavy machinery and equipment by recognizing the shape of the tunnel face or side wall using LiDAR (Laser Imaging Detection and Ranging, Light Detection and Ranging) technology (for example, the technology described in Non-Patent Document 1). LiDAR technology is a technology that measures the distance to an object by irradiating it with laser light and scanning it, and then uses that data to recognize its shape. [Prior art documents] [Patent Documents]

[0004] [Non-Patent Document 1] "Achieving Automated Operation of Measurement Equipment" ("Overview" section) [online], September 13, 2023, Nishimatsu Construction website, [Accessed October 25, 2024], Internet<URL: https: / / www.nishimatsu.co.jp / news / 2023 / post_91.html> [Overview of the project] [Problems that the invention aims to solve]

[0005] However, in the aforementioned LiDAR technology used to recognize the shape of the tunnel face or side wall, other heavy machinery and various underground installations placed inside the tunnel could act as obstacles, making it difficult to accurately grasp the shape of the tunnel face or side wall, and consequently, to accurately detect the direction angle.

[0006] Therefore, the present invention aims to achieve stable detection of the direction angle without being affected by obstacles in heavy machinery and equipment that autonomously travel in tunnels. [Means for solving the problem]

[0007] A direction angle detection device that detects the direction angle indicating the direction of travel of heavy machinery and equipment autonomously traveling inside a tunnel, Mounted on the aforementioned heavy machinery / device, the device has the function of irradiating laser light from a LiDAR at an arbitrary scanning angle, and within the optical scanning plane, which is a virtual plane perpendicular to the direction of travel of the heavy machinery / device and includes the irradiation center point of the laser light, the device irradiates the laser light from the irradiation center point toward the inside of the upper part of the tunnel at each predetermined angle change from the irradiation angle of the laser light toward the first side of the tunnel toward the irradiation angle of the laser light toward the second side opposite to the first side, and measures the behavior of the laser light until it is reflected back at the inner light irradiation position, thereby detecting the distance from the irradiation center point to the light irradiation position as the detection distance, and based on the set of values ​​of the detection distance detected for each scanning angle, the device detects the arch shape indicating the inner circumference line, The system includes a direction angle calculation unit that calculates the direction angle indicating the direction of travel of the heavy machinery / device, The arch shape detection unit measures the distance between the center point of the LiDAR scan and the side or top for each of the multiple measurement points, and detects the arch shape as a collection of the distance data acquired for each measurement point. A direction angle calculation unit pre-detects and stores a set of reference distance values, which are the distances detected by the arch shape detection unit for each of the scanning angles when the heavy machinery / device is pointed towards each of the multiple reference direction angles, as a reference arch shape corresponding to the reference direction angle. During autonomous driving of the heavy machinery / device, the unit determines the reference arch shape that is closest to the arch shape detected by the arch shape detection unit based on the set of detected distance values ​​across the multiple scanning angles, from among the multiple reference arch shapes that are pre-stored, each of which is a set of reference distance values ​​across the multiple scanning angles, and calculates the reference direction angle corresponding to the determined reference arch shape as the direction angle. A direction angle detection device equipped with the following features. [Effects of the Invention]

[0008] According to the present invention, it becomes possible to detect a stable directional angle in heavy machinery and equipment that autonomously travel within a tunnel. [Brief explanation of the drawing]

[0009] [Figure 1] This figure shows an example configuration of an autonomous heavy machinery / device for use inside a tunnel according to the embodiment. (a) is a perspective view, and (b) is a front view. [Figure 2] This is an explanatory diagram of the direction angle detection operation in the embodiment. [Figure 3] This is a diagram (part 1) showing an example of a reference arch shape. (a) shows the heavy machinery / equipment moving in the direction of tunnel excavation, (b) shows the heavy machinery / equipment moving 30 degrees to the left relative to the direction of tunnel excavation, and (c) shows the heavy machinery / equipment moving 45 degrees to the left relative to the direction of tunnel excavation. [Figure 4] This is a second diagram showing an example of a reference arch shape. (a) shows the case where the heavy machinery / equipment is moving 60 degrees to the left relative to the tunnel excavation direction, and (b) shows the case where the heavy machinery / equipment is moving 90 degrees to the left relative to the tunnel excavation direction. [Figure 5]This is an explanatory diagram (part 1) showing the case where the main body of heavy machinery / equipment is located in the center of the tunnel and the case where it is offset from the center. (a-1) and (a-2) show the case where it is in the center, and (b-1), (b-2) and (b-3) show the case where it is offset from the center. [Figure 6] This is a diagram illustrating the matching degree detection process. [Figure 7] This is an explanatory diagram (part 2) for cases where the main body of heavy machinery / equipment is offset from the center of the tunnel. (a) shows the case where the heavy machinery / equipment is moving in the direction of tunnel excavation, and (b) shows the case where the heavy machinery / equipment is moving θ degrees to the left with respect to the direction of tunnel excavation. [Figure 8] This is an explanatory diagram of the correction calculation process. [Figure 9] This figure shows an example of the hardware configuration of a direction angle detection device. [Figure 10] This flowchart shows an example of the basic processing for directional angle control. [Figure 11] This flowchart shows an example of detailed processing according to one embodiment of the arch shape detection process and direction angle calculation process. [Figure 12] This flowchart shows an example of further detailed processing according to one embodiment of the direction angle calculation process. [Figure 13] This is an explanatory diagram of another embodiment of the direction angle calculation process. [Figure 14] This flowchart shows an example of further detailed processing according to another embodiment of the direction angle calculation process. [Modes for carrying out the invention]

[0010] Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings. Figure 1 is a diagram showing an example configuration of a heavy machine / device 100 that autonomously travels inside a tunnel according to an embodiment of the present invention (hereinafter referred to as "this embodiment"). The heavy machine / device 100 shown in Figure 1(a) is equipped with various construction devices and control devices for constructing the tunnel 110, and autonomously travels inside the tunnel 110 in the direction of the tunnel face and the opposite direction of the tunnel entrance. Autonomous travel is performed using known control technologies such as SLAM, but the heavy machine / device 100 of this embodiment has a function to calculate the direction angle of travel of the heavy machine / device 100 and supply it to the SLAM control.

[0011] In Figure 1(a), the arch shape detection unit 101 is mounted on the heavy machinery / device 100 and detects the arch shape of the upper part 111 of the tunnel 110 using a LiDAR (Light Detection And Ranging) method.

[0012] A commercially available 2D LiDAR can be used as the arch shape detection unit 101. The arch shape detection unit 101 detects the distance from the irradiation center point 150 to the light irradiation position 140(#i) by measuring the behavior of the laser light 120(#i), which is ultraviolet light, visible light, near-infrared light, etc., from the irradiation center point 150 within a virtual plane 130 shown in Figure 1(b) (hereinafter referred to as the "optical scanning plane 130") that is perpendicular to the direction of travel A in Figure 1(a), including the irradiation center point 150, at the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) (N is a natural number). The behavior of the laser light 120(#i) from the irradiation center point 150 until it is reflected back at the light irradiation position 140(#i) inside the upper part 111 is measured, and the distance from the irradiation center point 150 to the light irradiation position 140(#i) is detected as the detection distance. The arch shape detection unit 101 then detects an arch shape representing the inner circumference line of the upper part 111 based on the set of detection distance values ​​detected for each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N). Here, the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) is assumed to change in predetermined angles (for example, {180° (π radians) ÷ (N+1)}° (radians)) in a counterclockwise direction around the irradiation center 150, for example, up to a scanning angle α(#N) = 180 degrees (π radians) (for example, in Figure 1(b)), where the scanning angle α(#1) of the laser beam 120(#1) directed from the irradiation center 150 toward one side (first side) 112a is 0 degrees (0 radians (rad)). In the following explanation, the above-mentioned scanning angle α(#i), laser beam 120(#i), and light irradiation position 140(#i), when i=1, 2, 3, 4, ..., N-1, and N, may be referred to as scanning angle α, laser beam 120, and light irradiation position 140, etc., without the notation "(#i)".

[0013] The direction angle calculation unit 102 is a computer mounted on the main body 103 of the heavy machinery / device 100 that executes a direction angle control processing program, which will be described later (see Figures 9, 10, 11, 12, and 14, which will be described later). Based on the relationship between the width between one side 112a and the other side 112b of the arch shape detected by the arch shape detection unit 101, which is a 2D LiDAR, and the cross-sectional width of the tunnel 110 which has been acquired in advance, the direction angle calculation unit 102 calculates the angle that the direction of travel A of the heavy machinery / device 100 (Figure 1(a)) makes with respect to the excavation direction of the tunnel 110 as the direction angle.

[0014] In addition to the above-mentioned devices for detecting the arch shape and direction angle related to the present invention, the heavy machinery / device 100 in Figure 1 may be equipped with, for example, various other sensors and control devices necessary for SLAM autonomous driving.

[0015] Figure 2 is an explanatory diagram of the direction angle calculation operation by the direction angle calculation unit 102 in Figure 1. Consider the case where the main body 103 of the heavy machinery / device 100 is located near the center of the cross-section of the tunnel 110 (the center of the line segment connecting one side 112a to the other side 112b within the cross-section). In this case, if the direction of travel 201 of the main body 103 (A in Figure 1(a)) coincides with the excavation direction 202 of the tunnel 110, the length of the line segment 203 connecting one side 112a where the laser beam 120(#1) with scanning angle α(#1) = 0° (0 radians) shown in Figure 1(b) intersects with one tunnel wall 210a, and the other side 112b where the laser beam 120(#N) with scanning angle α(#N) = 180° (π radians) shown in Figure 1(b) intersects with the other tunnel wall 210a, is the shortest distance between one tunnel wall 210a and the other tunnel wall 201b. Therefore, the arch shape indicated by the collection of detection distance values ​​220 (black circles "●" in Figure 2(a)) for each scanning angle α within the optical scanning surface 130 (see Figure 1(b)), detected by the arch shape detection unit 101, becomes as shown in Figure 2(a), and the distance between one side 112a and the other side 112b (length of line segment 203) becomes approximately equal to the cross-sectional width D of the tunnel 110.

[0016] On the other hand, as shown in Figure 2(b), for example, when the main body 103 of the heavy machinery / device 100 is located near the center of the cross-section of the tunnel 110, and the direction of travel 201 of the main body 103 (A in Figure 1(a)) is inclined at an angle θ with respect to the excavation direction 202 of the tunnel 110, the length of the line segment 203 where one side 112a and the other side 112b intersect will be as shown in the left side of Figure 2(b). Therefore, the arch shape shown by the set of detection distance values ​​220 (black circles "●" in Figure 2(b)) for each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) within the optical scanning surface 130 (see Figure 1(b)) detected by the arch shape detection unit 101 is as shown in Figure 2(b), and the distance L (length of line segment 203) between one side 112a and the other side 112b can be calculated by the calculation D / cos(θ) (where "cos" is the cosine function operation on the number in parentheses) with respect to the cross-sectional width D of the tunnel 110. Therefore, if the distance L between one side 112a and the other side 112b of the arch shape detected by the arch shape detection unit 101 can be detected, the direction angle θ that the direction of travel 201 (A in Figure 1(a)) of the heavy machinery / device 100 in Figure 1(a) makes with respect to the excavation direction 202 of the tunnel 110 in the case of Figure 2(b) can be calculated by the calculation shown in equation (1) below. Here, "acos()" is the arccosine function operation on the number in parentheses.

[0017] θ = acos(D / L) ... (1)

[0018] By providing the direction angle θ calculated in this way to the autonomous control system of the heavy machinery / device 100, it becomes possible to correctly perform SLAM autonomous control so that, for example, the direction of travel 201 of the heavy machinery / device 100 is oriented in the direction of excavation 202 of the tunnel 110.

[0019] The direction angle θ can be calculated using equation (1) above, with the interval L calculated from the arch shape detected by the arch shape detection unit 101. However, the calculation of the direction angle θ may be affected by obstacles such as air pipes or tall heavy machinery installed near the upper part 111. Nevertheless, since the assumed shape is simple (for example, an ellipse with a known size), according to this embodiment, interpolation processing is possible even if the detection distance value 220 is partially missing, assuming the arch shape of the tunnel 110.

[0020] Furthermore, in this embodiment, the following process is performed to enable stable calculation of the direction angle θ.

[0021] First, the arch shape detection unit 101 detects the distance between the irradiation center point 150 (Figure 1(b)) and each light irradiation position 140(#i) (i=1, 2, 3, 4, ..., N-1, N) (Figure 1(b)) on one side 112a, the other side 112b, or the upper part 111 as the detection distance for each of the multiple scanning angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) (Figure 1(b)) as described in Figure 1(b). Based on this set of detection distance values, it detects the arch shape that shows the circumferential line of the upper part 111 from one side 112a to the other side 112b of the tunnel 110.

[0022] Next, the direction angle calculation unit 102 pre-positions the main body 103 of the heavy machinery / device 100 at the center of the cross-section of the tunnel 110 at an arbitrary position within the tunnel 110. Then, at that center cross-section, when the main body 103 of the heavy machinery / device 100 is pointed towards each of a plurality of known reference direction angles, the arch shape detection unit 101 detects a set of reference distance values ​​301 for each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) as described in Figure 1(b), and stores these sets of reference arch shapes corresponding to each reference direction angle. Figures 3(a), (b), and (c), and Figures 4(a) and (b) show the arch shapes which are sets of reference distance values ​​301 for each scanning angle that the direction angle calculation unit 102 pre-stores when the known direction angles θ are 0°, 30°, 45°, 60°, and 90°, respectively. Of course, the direction angle θ is not limited to these angles.

[0023] When the heavy machinery / device 100 is autonomously moving, the direction angle calculation unit 102 determines the reference arch shape that best matches the arch shape detected by the arch shape detection unit 101 based on the set of detection distance values ​​220 over the above multiple scanning angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) from among a set of pre-stored reference arch shapes, such as Figure 3(a), (b), or (c), or Figure 4(a) or (b), and calculates the reference direction angle corresponding to the determined reference arch shape as the direction angle θ.

[0024] As mentioned above, the reference distance value 301 (see Figures 3 and 4) is detected by the arch shape detection unit 101 at the center of the cross-section of the tunnel 110. Therefore, for example, as shown in Figure 5(a-1), when the main body 103 of the heavy machinery / device 100 is near the center of the cross-section of the tunnel 110, the set of detected distance values ​​220 (black circles "●" in Figure 2(a), etc.) detected for the laser beam 120 (for example, 120a, 120b, 120c in Figure 5(a-2)) irradiated from the arch shape detection unit 101 on the main body 103 to the inside of the upper part 111 of the tunnel 110, closely matches the set of reference distance values ​​301 (see Figure 3 or Figure 4) that have an arch shape that matches it.

[0025] As described above, assuming that the main body 103 of the heavy machinery / device 100 is near the center of the cross-section of the tunnel 110, similar to the detection of the reference arch shape, the following method can be used to determine which reference arch shape matches the detected arch shape based on the set of detection distance values ​​220.

[0026] Specifically, when the heavy machinery / device 100 is autonomously moving, the direction angle calculation unit 102 calculates the difference between the detection distance value 220 corresponding to each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) detected by the arch shape detection unit 101 and the reference distance value 301 corresponding to that scanning angle α(#i) that represents one reference arch shape, for each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) as described in Figure 1(b). The sum of the squares of the differences calculated over multiple scanning angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) is calculated as the degree of match for that one reference arch shape.

[0027] Figure 6 is an explanatory diagram illustrating the detection operation of the degree of match between a set of detected detection distance values ​​220 and a set of reference distance values ​​301. For example, when the heavy machinery / device 100 is autonomously driving, the detection distance value 220 (black circle "●" in Figure 6) detected by the arch shape detection unit 101, which is a 2D LiDAR, is detected for, for example, 13 scanning angles α(#i) (i=1, 2, 3, 4, ..., N=13) along the semicircle along the upper part 111 between one side 112a and the other side 112b. Similarly, the reference distance value 301 (triangle mark "▲" in Figure 6) that has been previously detected by the arch shape detection unit 101 is also detected for, for example, 13 scanning angles α(#i) (i=1, 2, 3, 4, ..., N=13). Note that the number N=13 in the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) is merely an example for the sake of explanation; the actual number of N can be larger and set according to the required detection accuracy for a given direction angle θ.

[0028] The direction angle calculation unit 102 first calculates the difference d1 between the detection distance value 220 (●) and the corresponding reference distance value 301 (▲) for the first scanning angle α (#1) near one side 112a, within the XY plane of Figure 6 corresponding to the optical scanning surface 130 in Figure 1 or Figure 2. Similarly, the direction angle calculation unit 102 calculates d13 from the difference d2 between the detection distance value 220 and the corresponding reference distance value 301 for each of the second to thirteenth scanning angles α (#i) (i=2, 3, 4, ..., N=13).

[0029] Then, the direction angle calculation unit 102 calculates the sum of the squares of the 13 differences calculated as described above, according to the calculation shown in equation (2) below, and uses that as the degree of match for one of the reference arch shapes.

[0030] Match score = d1 2 +d2 2 +···+d13 2 ...(2)

[0031] For a typical number N, the degree of match can be calculated according to the operation shown in (3) below.

[0032] Match score = d1 2 +d2 2 +···+dN 2 ...(3)

[0033] The direction angle calculation unit 102 calculates the degree of match between the detected arch shape and each reference arch shape when the known direction angle θ is 0°, 30°, 45°, 60°, and 90°, respectively, as shown in Figures 3(a), (b), and (c), and Figures 4(a) and (b), which have been stored in advance. Then, it calculates the reference direction angle of the reference arch shape corresponding to the smallest of the respective degree of match as the direction angle θ of the heavy machinery / device 100.

[0034] However, as shown in Figure 5(b-1), for example, if the main body 103 of the heavy machinery / device 100 is located near the tunnel side wall, away from the center of the tunnel cross-section, the detection distance value 220 detected for each scanning angle from the arch shape detection unit 101 on the main body 103 to the inside of the upper part 111 of the tunnel 110 will be excessively large compared to the reference distance value 301 for the same scanning angle with a matching arch shape at a scanning angle 120a′ where the distance from the main body 103 to the inside of the upper part 111 is far, as shown in Figure 5(b-2). Conversely, in scanning directions 120c′ where the distance from the main body 103 to the inside of the upper part 111 is short, the detection distance value 301 for the same scanning direction with a matching arch shape will be excessively large compared to the reference distance value 301 for the same scanning direction with a matching arch shape. Therefore, for example, as shown in Figure 7(a) (same as Figure 5(b-1)), if the main body 103 of the heavy machinery / device 100 is located away from the center of the cross-section of the tunnel 110, the set of detection distance values ​​220 detected by the arch shape detection unit 101 (the set of black circles "●" in Figure 7(a)) will be significantly different from the set of reference distance values ​​301 that have an arch shape that matches it (the arch shown in gray in Figure 7(a)), and the degree of match calculated by equation (2) above will also not be the correct value. Figure 7(a) shows the case where the direction of travel 201 of the heavy machinery / device 100 coincides with the excavation direction 202 of the tunnel 110, but the same applies when the direction of travel 201 makes a directional angle θ with respect to the excavation direction 202, as shown in Figure 7(b).

[0035] Therefore, in this embodiment, we first consider correcting the detection polar coordinate data, which for each scanning angle is the scanning angle on a two-dimensional detection polar coordinate system with the laser beam irradiation center point 150 as the origin and the detection distance value 220 detected for that scanning angle, into corrected polar coordinate data, which is the corrected scanning angle on a two-dimensional tunnel central polar coordinate system with the tunnel central part as the origin and the corrected detection distance corresponding to that corrected scanning angle as the coordinate values.

[0036] As shown in Figure 2 above, when the main body 103 of the heavy machinery / device 100 is located near the center of the tunnel 110's cross-section, the distance from one side 112a of the tunnel wall 210a to the main body 103 is approximately equal to the distance from the other side 112b of the tunnel wall 201b to the main body 103, whether the direction of travel 201 of the main body 103 coincides with the excavation direction 202 of the tunnel 110 as shown in Figure 2(a), or whether it is inclined with respect to the excavation direction 202 as shown in Figure 2(b). In other words, the midpoint of the line segment connecting one side 112a and the other side 112b of the tunnel 110 on the optical scanning surface 130 can be considered as the center of the tunnel 110's cross-section (hereinafter referred to as the "tunnel center"). In contrast, if the main body 103 of the heavy machinery / device 100 is located away from the center of the cross-section of the tunnel 110, the distance from one side 112a of the tunnel wall 210a to the main body 103 will not be equal to the distance from the other side 112b of the tunnel wall 201b to the main body 103.

[0037] Based on these relationships, for example as shown in Figure 8(a), on the scanning plane 130 perpendicular to the direction of travel of the main body 103 of the heavy machinery / device 100, the midpoint of the line segment connecting one side 112a and the other side 112b B800 can be defined as the central portion of the tunnel, a point separated from the central portion of the tunnel on this line segment by a distance dL (hereinafter referred to as "correction offset value dL") A 800 can be defined as the current irradiation center point 150 of the laser beam. Then, the correction offset value dL can be calculated as a value obtained by dividing by 2 the difference resulting from subtracting the detection distance value 220(#N) detected by the laser beam 120(#N) irradiated toward the side portion 112b of the other tunnel wall 210b (see FIG. 2) at a scanning angle α(#N)=180° (π radians) from the detection distance value 220(#1) detected by the laser beam 120(#1) irradiated toward the side portion 112a of one tunnel wall 210a (see FIG. 2) at a scanning angle α(#1)=0° (0 radians) shown in FIG. 1(b), through the calculation shown in the following equation (4).

[0038] dL=(detection distance value 220(#1) - detection distance value 220(#N)) / 2 ···(4)

[0039] Here, the laser beam 120 emitted by the arch shape detection unit 101 (the laser beam in FIG. 8(a) A 120) has a scanning angle of A α, and the detection distance value 220 detected there is A r, then the corresponding light irradiation position 140 is the point that is the irradiation center point 150 A 800 as the origin on a two-dimensional polar coordinate system (hereinafter referred to as "detection polar coordinate system A"), coordinate data ( A α, A r) (hereinafter referred to as "detection polar coordinate data ( A α, A r)") can be expressed by the following.

[0040] When this same light irradiation position 140 is corrected to coordinate data on a two-dimensional polar coordinate system (hereinafter referred to as "tunnel center polar coordinate system B") with the point B 800 as the origin which is the central portion of the tunnel, the light irradiation position 140 has coordinate data ( B α, B r) (hereinafter referred to as "corrected polar coordinate data ( B α,B It can be represented by (r)) (as written). Here, B α is the point in the center of the tunnel. B Assuming that the irradiation center point is 150, the laser beam 120 (the laser beam in Figure 8(a)) is located from there. B Assuming that 120) is directed towards the light irradiation position 140, the scanning angle (hereinafter referred to as "corrected scanning angle") B It can be defined as "α" (as indicated). B r is the laser beam B The value of the detection distance assumed to be detected by 120 (hereinafter referred to as the "corrected detection distance value") B It can be defined as "r" (written as "r").

[0041] In this embodiment, the coordinate data on the detected polar coordinate system A is obtained as follows: A α, A r) Corrected polar coordinate data on the tunnel center polar coordinate system B ( B α, B Correct to r).

[0042] First, the coordinate data on the detection polar coordinate system A ( A α, A r) is obtained by the calculations shown in equations (5) and (6) below, as shown in Figure 8(a). A X axis and A Coordinate data on a 2D Cartesian coordinate system indicated by the Y axis ( A x, A It can be converted to y). Here, cos() and sin() are the cosine and sine functions, respectively, applied to the number in parentheses.

[0043] A x= A r×cos( A α) ··(5) A y= A r × sin( A α) ··(6)

[0044] Similarly, corrected polar coordinate data on the tunnel's central polar coordinate system B ( B α, Br) is obtained by the calculations shown in equations (7) and (8) below, as shown in Figure 8(a). B X axis and B Coordinate data on a 2D Cartesian coordinate system indicated by the Y axis ( B x, B It can be converted to y).

[0045] B x= B r×cos( B α) ··(7) B y= B r × sin( B α) ··(8)

[0046] From the relationship shown in Figure 8(a), the operations shown in equations (9) and (10) below hold true.

[0047] B x= A x-dL ··(9) B y= A y ··(10)

[0048] By substituting equations (5) and (6) into equations (9) and (10), the operations shown in equations (11) and (12) below are obtained.

[0049] B x= A x-dL= A r×cos( A α)-dL ··(11) B y= A y= A r × sin( A α) ··(12)

[0050] On the other hand, coordinate data on a Cartesian coordinate system ( B x, B From y), corrected polar coordinate data on the tunnel's central polar coordinate system B ( B α, B r) Value of the corrected detection distance Br can be calculated by the operation of the following formula (13). Here, sqrt() is a square root operation on the numerical value in the parentheses.

[0051] B r=sqrt( B x 2 + B y 2 ) ··(13)

[0052] Further, from formula (7) and formula (13), the operation represented by the following formula (14) is derived.

[0053] B α=acos( B x / B r) =acos( B x / sqrt( B x 2 + B y 2 )) ··(14)

[0054] Furthermore, by substituting formula (9) and formula (10) obtained by substituting formula (5) and formula (6) into the above formula (13) and formula (14) respectively, the operations represented by the following formula (15) and formula (16) are derived.

[0055] B α=acos(( A x-dL) / sqrt(( A x-dL) 2 + A y 2 )) =acos(( A r×cos( A α)-dL) / sqrt(( A r×cos( A α)-dL) 2 +( A r×sin( A α)) 2 )) ···(15) B r=sqrt(( A r×cos( A α)-dL) 2 +( A r × sin( A α)) 2 ) ··(16)

[0056] The arch shape detection unit 101 is A Detected at each scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) where α is the scan angle. A For a set of detection distance values ​​r = 220, the coordinate data on the detection polar coordinate system A is obtained by the correction calculation shown in equations (15) and (16) above. A α, A The set of r) is corrected polar coordinate data on the tunnel central polar coordinate system B ( B α, B It becomes possible to correct the arch shape to match the set of r). That is, the arch shape detection unit 101 can correct the arch shape detected as distorted because the laser beam 120 is irradiated from the irradiation center point 150 which is shifted from the center of the tunnel, so that it matches the reference arch shape.

[0057] The correction calculations shown in equations (15) and (16) above are performed using coordinate data on the detected polar coordinate system A. A α, A r) and corrected polar coordinate data on the tunnel center polar coordinate system B ( B α, B The same holds true even when the relationship in r) is as shown in Figure 8(b). Furthermore, the above correction calculation is performed at the irradiation center point. A 800 is in the center of the tunnel B The same principle holds true not only when the other side 112b (left side in the drawing) is located relative to 800, as shown in Figure 8(a) or Figure 8(b), but also when it is located on the side of one side 112a (right side in the drawing), with only the sign of the correction offset value dL changing.

[0058] However, as is clear from Figure 8, the corrected polar coordinate data obtained by the above correction calculation ( B α, B Corrected scanning angle in the set of r) B The set of α is the coordinate data on the original detection polar coordinate system A (A α, A Scan angle in the set of r) A This is different from the scan angle α(#i) (i=1, 2, 3, 4, ..., N-1, N), which is a set of α values. In this state, the set of 301 reference distance values ​​for multiple arch shapes that are pre-stored for each scan angle α(#i) is used to correct the detection distance value. B It cannot be directly compared to the set of r.

[0059] Therefore, in this embodiment, the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) that we want to calculate by interpolation is called N. A The interpolated detection distance value obtained by interpolation calculation corresponding to α and its scanning angle. B Interpolated polar coordinate data containing r' as coordinate values ​​( A α, B r') is calculated using the interpolation operation shown below.

[0060] First, the interpolated polar coordinate data that we want to calculate using interpolation calculation ( A α, B Scan angle of r') A The two corrected scan angles closest to the scan angle with α in between are obtained by the correction calculation processes shown in equations (15) and (16) above. B α1 and B Two corrected polar coordinate data sets, each containing α2 as a coordinate value, are respectively ( B α1, B r1) and ( B α2, B Let r2) be the case here, B α2> B Let's call it α1. In this embodiment, the corrected polar coordinate data of the two points above ( B α1, B r1) and ( B α2, B Scan angle between r2) A radius at α B r' is calculated using the circular interpolation operation shown below.

[0061] In this embodiment, the value of the corrected detection distance BWe assume that the interpolation curve used to calculate r follows a circle with constant curvature. This assumption is reasonable because the upper part 111 of the tunnel 110 is often a semicircular arc. However, if the cross-section of the tunnel 110 is close to a rectangular prism and the upper part is a straight line, other interpolation calculations may be performed.

[0062] First, the corrected polar coordinate data for the two points mentioned above ( B α1, B r1) and ( B α2, B r2) is calculated by the operations shown in equations (17) / (18) and (19) / (20) below, respectively, as shown in Figure 8. B X axis and B Corrected Cartesian coordinate data of two points on a 2D Cartesian coordinate system indicated by the Y-axis ( B x1, B y1) and ( B x2, B Convert to y2).

[0063] B x1= B r1×cos( B α1) ··(17) B y1= B r1 × sin( B α1) ··(18) B x2= B r² × cos( B α2) ··(19) B y2 = B r² × sin( B α2) ··(20)

[0064] Next, the corrected Cartesian coordinate data of the two points calculated by the calculation from equation (17) to equation (20) above ( B x1, B y1) and ( B x2, B Midpoint coordinate data M(M) x M y ) is calculated by the operations shown in equations (21) and (22) below.

[0065] M x =( B x1+ B x2) / 2 ··(21) M y =( B y1+ B y2) / 2 ··(22)

[0066] Substituting equations (17) to (20) into the above equations (21) and (22) respectively gives the following equations (23) and (24).

[0067] M x =( B r1×cos( B α1)+ B r2×cos( B α2)) / 2 ··(23) M y =( B r1×sin( B α1)+ B r2×sin( B α2)) / 2 ··(24)

[0068] Next, the distance d between two corrected orthogonal coordinate data points ( B x1, B y1) and ( B x2, B y2) calculated by the operations of equations (17) to (20) is calculated by the following equation (25).

[0069] d=sqrt(( B x2- B x1) 2 +( B y 2- B y1) 2 ) ··(25)

[0070] Substituting equations (17) to (20) into the above equation (25) gives the following equation (26).

[0071] d=sqrt(( B r2×cos( B α2)- B r1×cos( B α1))2 +( B r2×sin( B α2)- B r1×sin( B α1)) 2 ) ··(26)

[0072] Further, the radius R of the circle representing the above-described interpolation curve is calculated by the following formula (27).

[0073] R=d / (2×sin(( B α 2- B α1) / 2)) ··(27)

[0074] Substituting formula (26) into the above formula (27) gives the following formula (28).

[0075] R=sqrt(( B r2×cos( B α2)- B r1×cos( B α1)) 2 +( B r2×sin( B α2)- B r1×sin( B α1)) 2 ) / (2×sin(( B α2- B α1) / 2)) ··(28)

[0076] Next, the direction vector v of the chord, which is a line segment connecting two corrected orthogonal coordinate data ([...]) calculated by the calculation from the above formula (17) to formula (20), B x1, B y1) and ( B x2, B y2) is calculated by the following formula (29).

[0077] v=( B x 2- B x1, B y2- B y1) ··(29)

[0078] Then, a unit normal vector n(n) perpendicular to the direction vector v. x ,n y ) is calculated by equations (30) and (31) below.

[0079] n x =-( B y2- B y1) / d ··(30) n y =( B x 2- B x1) / d ··(31)

[0080] Substituting equations (20) and (26) from equation (17) into equations (30) and (31) respectively yields equations (32) and (33) below.

[0081] n x =-( B r² × sin( B α2)- B r1 × sin( B α1)) / sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) ··(32) n y =( B r² × cos( B α2)- B r1×cos( B α1)) / sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)-B r1 × sin( B α1)) 2 ) ··(33)

[0082] The midpoint M(M) calculated by equations (23) and (24) above x M y The distance h from ) to the center of the circle showing the interpolation curve mentioned above is calculated by equation (34) below.

[0083] h=sqrt(R 2- (d / 2) 2 ) ··(34)

[0084] Substituting equations (26) and (28) into equation (34) above, we obtain equation (35) below.

[0085] h=sqrt((sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) / (2×sin(( B α2- B α1) / 2))) 2 -(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) / 4) ··(35)

[0086] And the coordinates C(C) of the center of the circle above x ,C y ) is calculated by equations (36) and (37) below.

[0087] C x = M x + h × n x ··(36) C y = M y + h × n y ··(37)

[0088] Substituting equations (23), (24), (32), (33) and (35) into the above equations (36) and (37) respectively gives the following equations (38) and (39). C x =( B r1 × cos( B α1)+ B r2 × cos( B α2)) / 2 +sqrt((sqrt(( B r2 × cos( B α2)- B r1 × cos( B α1)) 2 +( B r2 × sin( B α2)- B r1 × sin( B α1)) 2 ) / (2 × sin(( B α2- B α1) / 2))) 2 -(( B r2 × cos( B α2)- B r1 × cos( B α1)) 2 +( B r2 × sin( B α2)- B r1 × sin( B α1)) 2 ) / 4) ×(-( B r2 × sin( B α2)- B r1 × sin( B α1)) / sqrt(( B r2 × cos(B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 )) ··(38) C y =( B r1×sin( B a1)+ B r2×sin( B a2)) / 2 +sqrt((sqrt(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 ) / (2×sin(( B a2- B a1) / 2))) 2 -(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 ) / 4) ×(( B r2×cos( B a2)- B r1×cos( B a1)) / sqrt(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( Br² × sin( B α2)- B r1 × sin( B α1)) 2 )) ··(39)

[0089] The center of the circle C(C) calculated by equations (38) and (39) and (28) x ,C y Using the radius R, the scan angle to be calculated by interpolation is located on the arc of this circle. A α and interpolation detection distance B Interpolated polar coordinate data with r' as the coordinate value ( A α, B Figure 8 corresponding to r') B X axis and B Interpolated orthogonal coordinate data on a 2D orthogonal coordinate system indicated by the Y axis ( B x′, B y') is calculated using equations (42) and (43) below.

[0090] B x′=C x +R×cos( A α) ··(42) B y'=C y +R × sin( A α) ··(43)

[0091] Interpolated Cartesian coordinate data calculated by equations (42) and (43) B x′, B Based on y'), the corresponding interpolated polar coordinate data ( A α, B Interpolated detection distance of r') B r' can be calculated using the following formula (44).

[0092] B r'=sqrt( B x' 2 + B y' 2 ) =sqrt((C x +R×cos( A α)) 2 +(Cy +R × sin( A α)) 2 )··(44)

[0093] By substituting equations (28), (38), and (39) into equation (44), we obtain equation (45) below.

[0094] B r' = sqrt((( B r1×cos( B α1)+ B r² × cos( B α2)) / 2 +sqrt((sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) / (2×sin(( B α2- B α1) / 2))) 2 -(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) / 4) ×(-( B r² × sin( B α2)- B r1 × sin( B α1)) / sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( Br2×sin( B a2)- B r1×sin( B a1)) 2 )) +(sqrt(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 ) / (2×sin(( B a2- B α1) / 2)))×cos( A a)) 2 +(( B r1×sin( B a1)+ B r2×sin( B a2)) / 2 +sqrt((sqrt(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 ) / (2×sin(( B a2- B a1) / 2))) 2 -(( B r2×cos( B a2)- B r1×cos( B a1)) 2 +( B r2×sin( B a2)- B r1×sin( B a1)) 2 ) / 4) ×(( Br² × cos( B α2)- B r1×cos( B α1)) / sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 )) +(sqrt(( B r² × cos( B α2)- B r1×cos( B α1)) 2 +( B r² × sin( B α2)- B r1 × sin( B α1)) 2 ) / (2×sin(( B α2- B α1) / 2)))×sin( A α)) 2 )··(45)

[0095] In this embodiment, the arch shape detection unit 101 (see Figure 1) uses the corrected polar coordinate data as described above. B α, B After calculating the set of r), scan angle A While sequentially changing α from α(#1) to α(#N), the two correction scan angles closest to that scan angle are... B α1 and B Two corrected polar coordinate data sets that each include α2 as a coordinate value. B α1, B r1) and ( B α2, B First, r2) is determined. Then, the arch shape detection unit 101 performs interpolation calculations shown in equation (45) using the coordinate values ​​of the two coordinate data, thereby determining the scanning angle. A Interpolated detection distance value at αB Calculate r', and the scan angle calculated in this way A Interpolated detection distance values ​​for each α B The arch shape can be detected as a set of r' values.

[0096] When the heavy machinery / device 100 is autonomously moving, the direction angle calculation unit 102 (see Figure 1) determines whether to match each of the multiple reference arch shapes that have been stored in advance, and the set of reference distance values ​​301 of the reference arch shape is used by the arch shape detection unit 101 to calculate interpolated polar coordinate data through the above interpolation calculation process. A α, B The corrected detection distance values ​​included in the set of r') B The above determination is made by matching it with the set r. As a result, in this embodiment, the interpolation detection distance value B The scan angle of both r' and the reference distance value 301 A When α matches, it becomes possible to perform a proper comparison.

[0097] The direction angle calculation unit 102 performs the following comparison process as one embodiment of the above comparison. That is, in the above determination, the direction angle calculation unit 102 performs the scanning angle A For each α (=α(#i)(i=1, 2, 3, 4, ..., N-1, N)), the scanning angle is calculated by interpolation calculation in the arch shape detection unit 101. A Interpolated polar coordinate data containing α as a coordinate value ( A α, B Coordinate values ​​of the interpolated detection distance of r') B r′ and its scanning angle showing a single reference arch shape A The difference between α and the reference distance value 301 is calculated. In this calculation process, the detection distance value 220 (●) in Figure 6 above is used to determine the interpolated detection distance (coordinate) value. B Simply replace it with r'. Then, the direction angle calculation unit 102 calculates all scan angles AThe sum of the squares of the difference di (i=1, 2, 3, 4, ..., N-1, N) calculated over the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) is calculated as the match degree by the calculation process shown in equation (3) above in the explanation of Figure 6. The reference arch shape that calculated the smallest match degree among the multiple reference arch shapes stored in advance is determined, and the reference direction angle corresponding to that determined reference arch shape is calculated as the direction angle of the heavy machinery / device 100.

[0098] Figure 9 shows an example of the hardware configuration of the control circuit and direction angle calculation unit 102 of the arch shape detection unit 101 in Figure 1. This hardware has the configuration of a general computer, preferably an embedded computer. The hardware in Figure 9 has a configuration in which a CPU (processor) 901, ROM (read-only memory) 902, RAM (random access memory) 903, and SSD (solid state drive) storage device 904 are interconnected by a system bus 905.

[0099] The CPU 901 executes the direction angle control program stored in the ROM 902 while using the RAM 903 as work memory.

[0100] Figure 10 is a flowchart showing an example of the basic processing of the directional angle control process that the CPU 901 in Figure 9 executes by reading the directional angle control program from ROM 902 to RAM 903.

[0101] First, the CPU 901 performs arch shape detection processing at regular intervals (for example, several times per second) using the arch shape detection unit 101, which is a 2D LiDAR, as described above in Figures 1(b) and 2. In this arch shape detection processing, the CPU 901 detects an arch shape with a detection distance value 220 (see Figure 2) at multiple scanning angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) (step S1001).

[0102] Next, the CPU 901 performs a direction angle calculation process. In this direction angle detection process, the CPU 901 determines the reference arch shape that is closest to the arch shape detected in step S1001 based on the set of detected distance values ​​220, from among the multiple reference arch shapes indicated by the set of multiple reference distance values ​​301 that have been stored in advance, and outputs the reference direction angle corresponding to the determined reference arch shape as the direction angle θ of the heavy machinery / device to the SLAM process (step S1002). After that, CPU901 returns to the process of step S1001 and repeats the process.

[0103] Figure 11 is a flowchart showing an example of detailed processing according to one embodiment of the arch shape detection process in step S1001 of Figure 10 and the direction angle calculation process in step S1002 of Figure 10.

[0104] Figure 11(a) is a flowchart showing an example of detailed processing according to one embodiment of the arch shape detection process in step S1001 of Figure 10.

[0105] In Figure 11(a), the CPU 901 first detects an arch-shaped detection distance value 220 (see Figure 2) for each of several scanning angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) (step S1101).

[0106] Next, the CPU 901 calculates the correction offset value dL by the calculation shown in equation (4) above (step S1102).

[0107] Next, the CPU 901 changes the value of variable i, stored in RAM 903, sequentially from 1 in step S1103, and while it is determined in step S1105 that the value of variable i has not reached N and there are other scanning angles (determined as YES), it repeatedly executes the correction calculation process in step S1104 while selecting the scanning angle α(#i) in step S1103.

[0108] In step S1104, the CPU 901 determines the scan angle α(#i) selected in step S1103 = Aα and its scanning angle in step S1101 A The detection distance value detected relative to α is 220 = A Using r, the correction calculation process shown in equations (15) and (16) above is performed.

[0109] Through the correction calculation process over the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) by repeating the above steps S1102 to S1105, as described above, the coordinate data on the detected polar coordinate system A ( A α, A The set of r) is corrected polar coordinate data on the tunnel central polar coordinate system B ( B α, B It is corrected to the set of r).

[0110] Next, the CPU 901 changes the value of the aforementioned variable i sequentially from 1 in step S1106, and while it is determined in step S1108 that the value of variable i has not reached N and there are other scan angles (determined as YES), it repeatedly executes the interpolation calculation process in step S1107 while selecting the scan angle α(#i) in step S1106.

[0111] In step S1107, the CPU 901 determines the scan angle α(#i) selected in step S1103 = A The two correction scan angles closest to α B α1 and B Two corrected polar coordinate data sets that each include α2 as a coordinate value. B α1, B r1) and ( B α2, B Using r2), the arc interpolation calculation shown in equation (45) is performed to obtain the scan angle selected in step S1103. A Interpolated detection distance value at α B Calculate r'.

[0112] Through the interpolation calculation process over the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) by repeating the steps S1106 to S1108 described above, the scanning angle AInterpolated detection distance values ​​for each α B The arch shape is detected as a set of r' values.

[0113] Figure 11(b) is a flowchart showing an example of detailed processing according to one embodiment of the direction angle calculation process in step S1002 of Figure 10, based on one embodiment of the arch shape processing in Figure 11(a).

[0114] In the direction angle calculation process of step S1109 in Figure 11(b), which is a detailed process of step S1002 in Figure 10, the CPU 901, when performing a determination to match each of the multiple reference arch shapes stored in advance during autonomous driving of the heavy machinery / device 100 (step S1002 in Figure 10), calculates interpolated polar coordinate data from the set of reference distance values ​​301 of the reference arch shape in the arch shape detection process (step S1001 in Figure 10) using the correction / interpolation calculation process shown in the flowchart of Figure 11(a). A α, B The corrected detection distance values ​​included in the set of r') B The above determination is made by matching it with the set r.

[0115] Figure 12 is a flowchart showing an example of further detailed processing according to one embodiment in step S1109 of Figure 11, which is the detailed processing of the direction angle calculation process in step S1002 of Figure 10.

[0116] In Figure 12, the CPU 901 first selects a reference arch shape in step S1201, and while it is determined in step S1206 that there are other reference arch shapes (determined as YES), it repeatedly executes the series of processes from step S1202 to step S1205. As illustrated in Figures 3 and 4, the reference arch shape is stored in advance, for example, in the ROM 902 in Figure 9, as a set of reference distance values ​​301 for multiple scanning angles, along with the reference direction angle. Alternatively, it may be measured on-site and stored in the SSD storage device 904 in Figure 9.

[0117] In the series of processes described above, CPU901 first changes the value of the aforementioned variable i sequentially from 1 in step S1202, and while it is determined in step S1204 that the value of variable i has not reached N and there are other scan angles (determined as YES), it repeatedly executes the difference calculation process in step S1203 while selecting the scan angle α(#i) in step S1202.

[0118] In step S1203, the CPU 901 determines the scan angle α(#i) selected in step S1202 = A For each α, the scanning angle is calculated by the interpolation calculation process in step S1107 of Figure 11(a), which is the detailed processing in the arch shape detection process in step S1001 of Figure 10. A Interpolated polar coordinate data containing α as a coordinate value ( A α, B Coordinate values ​​of the interpolated detection distance of r') B r' and its scanning angle indicate the reference arch shape selected in step S1201. A A difference calculation process is performed to calculate the difference between the reference distance value 301 corresponding to α. In this calculation process, the detection distance value 220 (●) in Figure 6 above is used to calculate the interpolated detection distance (coordinate) value. B Simply replace it with r'.

[0119] If, after completing the difference calculation process for all scan angles α(#i) (i=1, 2, 3, 4, ..., N-1, N) for the currently selected reference arch shape in step S1201, and the determination in step S1204 is NO, then the CPU 901 will calculate all scan angles calculated in the iterative process of step S1203 for the current reference arch shape. A The sum of the squares of the differences di (i=1, 2, 3, 4, ..., N-1, N) calculated over the scanning angle α(#i) (i=1, 2, 3, 4, ..., N-1, N) is calculated as the degree of match to the current reference arch shape by the calculation process shown in equation (3) above (step S1205).

[0120] Once the calculation of the degree of match is completed for all reference arch shapes and the determination in step S1206 is NO, the CPU 901 determines the reference arch shape with the smallest degree of match among those calculated in step S1205 as the determination result in step S1002 in Figure 10 (step S1207).

[0121] Figure 13 is an explanatory diagram of another embodiment of the direction angle calculation process. For example, consider the case where there is an obstacle 1301 inside the tunnel 110, as illustrated in Figure 13. In this case, the sum of squares of the differences calculated in step S1205 based on the difference calculation process in step S1203 of Figure 12 described above will have larger difference values ​​d1, d2, d3, etc. for the portion with the obstacle 1301 than the original values, resulting in a worsening of the match. As a result, the difference values ​​d1', d2', d3', etc. with the reference distance value 301 related to the incorrect reference arch shape will be small, and the match will be calculated as such, potentially resulting in a match with the incorrect reference arch shape.

[0122] Therefore, in step S1109 of Figure 11, which is a detailed processing step of the direction angle calculation process in step S1002 of Figure 10, a flowchart of an example of further detailed processing according to another embodiment that solves the problem in Figure 13 is shown in Figure 14. In Figure 14, the iterative control processing of steps S1201 and S1206 for the reference arch shape and the scanning angle α(#i) = A The repeated control processing of steps S1202 and S1204 for α is the same as in the case of Figure 12. Also, the selected scanning angle A Interpolated detection distance values ​​for each α B Step S1203, which performs the process of calculating the difference between r' and the reference distance value 301, is the same as in the case of Figure 12.

[0123] In the detailed processing example shown in Figure 14, first, a variable representing the degree of match, which is stored in, for example, RAM 903 corresponding to the reference arch shape selected in step S1201, is reset to 0 (step S1401).

[0124] Next, the CPU 901 determines whether the difference value calculated in step S1203 is smaller than a predetermined threshold (step S1402). If the determination is YES, it increments the value of the aforementioned match degree variable on RAM 903, for example, which corresponds to the selected reference arch shape, by 1 (step S1403). If the determination in step S1402 is NO, the CPU 901 does not execute step S1403.

[0125] All scan angles A When the matching degree control processing in steps S1203, S1403, and S1404 is completed over the scanning angle α (#i) (i = 1, 2, 3, 4, ..., N-1, N), and the determination in step S1204 is NO, the CPU 901 stores the value of the matching degree variable, which has been updated on RAM 903 in accordance with the selected reference arch shape, in another memory area of ​​RAM 903 in accordance with that reference arch shape (step S1405).

[0126] Then, when the above series of processes is completed for all reference arch shapes and the determination in step S1206 is NO, the CPU 901 determines in step S1405, for example, the reference arch shape with the highest match degree among the match degrees stored for each reference arch shape in RAM 903, as the determination result in step S1002 in Figure 10.

[0127] As described above, in the example processing shown in the flowchart of Figure 14, the reference arch shape with a larger number of scan angles that match better will have a lower interpolation detection distance value. B By appropriately matching the arch shape detected as a set of r', it becomes possible to increase resistance to obstacles 1301 as exemplified in Figure 13.

[0128] In the interpolation calculation process in the embodiments described above, interpolation was performed for each scan angle from the two correction scan angles closest to that scan angle, straddling the scan angle. However, the present invention is not limited to this, and interpolation may be performed from three or more correction scan angles closest to that scan angle, straddling the scan angle. Also, in the embodiments described above, two correction polar coordinate data ( Bα1, B r1) and ( B α2, B Scan angle between r2) A radius at α B When calculating r' using circular interpolation, it was assumed that the interpolation curve follows a circle with constant curvature. Generally, this provides a sufficient approximation, but for a more accurate approximation, the circular interpolation calculation may be performed assuming that the interpolation curve based on the tunnel's cross-sectional shape follows an ellipse.

[0129] In the embodiments described above, if information is needed regarding whether the heavy machinery / device 100 is facing the tunnel face or the tunnel entrance, along with the direction angle θ, this can be determined, for example, from the work situation at that time. For example, when the heavy machinery / device 100 is returning to its parking spot after finishing work at the tunnel face, it can be determined that it is facing the tunnel entrance.

[0130] In the embodiments described above, if information is needed as to whether the direction of travel 201 in Figure 2 is shifted to the left or right relative to the excavation direction 202, this can be determined, for example, by observing the increment in the direction angle θ when the heavy machinery / device 100 is intentionally swung from side to side. For example, if the direction angle θ increases when swung slightly to the left and decreases when swung slightly to the right, it can be determined that the heavy machinery / device 100 was shifted to the left relative to the excavation direction 202. In this case, it is possible to calculate whether the direction angle θ increases or decreases from the widths D and L of the tunnel 110, or it is possible to determine whether it is shifted to the left or right by observing the change in the estimated angle through the arch-shaped matching in Figures 3, 4, and 6. When determining left or right by matching, it is preferable to increase the number of estimated angles and narrow the interval between them.

[0131] According to the embodiment described above, it is possible to operate the device at a lower cost and with less load on the control device (computer) than conventional measurement methods such as 3D LiDAR that are commonly used. [Explanation of Symbols]

[0132] 100 Heavy machinery / equipment 101 Arch shape detection unit 102 Direction Angle Calculation Unit 103 Main Unit 110 Tunnel 111 Top 112a One side 112b The other side 120 laser beams 130 Optical scanning surface 140 Light irradiation position 150 Irradiation center point 201 Direction of travel 202 Excavation direction 203 Direction perpendicular to the direction of travel 201 210a One tunnel wall 210b The other tunnel wall 220 detection distance value 301 Reference distance value 302 Cutting face A 800 A point located at a distance of dL from the center of the tunnel. B 800 Midpoint on the line segment connecting one side 112a and the other side 112b 901 CPU 902 ROM 903 RAM 904 SSD storage 905 System Bus 1301 Obstacle Steps S1001, S1002, S1101 to S1109, S1201 to S1207, S1401 to S1406 α, A α, scanning angle B α-corrected scanning angle B r is the value of the corrected detection distance. B r' Interpolated detection distance value d1,···, dN difference dL correction offset value

Claims

1. A direction angle detection device that detects the direction angle indicating the direction of travel of heavy machinery and equipment autonomously traveling inside a tunnel, Mounted on the aforementioned heavy machinery / device, and equipped with the function of irradiating a laser beam using LiDAR at an arbitrary scanning angle, the device irradiates the laser beam from the irradiation center point toward the inside of the upper part of the tunnel at each predetermined angle change in the optical scanning plane, which is a virtual plane perpendicular to the direction of travel of the heavy machinery / device and includes the irradiation center point of the laser beam, and measures the behavior of the laser beam until it is reflected back at the inner light irradiation position to detect the distance from the irradiation center point to the light irradiation position as the detection distance, and detects the arch shape indicating the inner circumference line based on the set of values ​​of the detection distance detected for each scanning angle, A direction angle calculation unit pre-detects and stores a set of reference distance values, which are the distances detected by the arch shape detection unit for each of the scan angles when the heavy machinery or device is pointed towards each of the multiple reference direction angles, as a reference arch shape corresponding to the reference direction angle. During autonomous driving of the heavy machinery or device, the unit determines which of the pre-stored multiple reference arch shapes, which are sets of reference distance values ​​across the multiple scan angles, is closest to the arch shape detected by the arch shape detection unit based on the set of detected distance values ​​across the multiple scan angles, and calculates the reference direction angle corresponding to the determined reference arch shape as the direction angle. A direction angle detection device equipped with the following features.

2. The direction angle calculation unit places the heavy machinery and equipment at the center of the tunnel cross-section and detects and stores the reference arch shape for each reference direction angle. The arch shape detection unit is Within the optical scanning plane, for each scanning angle, the detection polar coordinate data, whose coordinate values ​​are the scanning angle and the detection distance detected relative to the scanning angle on a two-dimensional detection polar coordinate system with the irradiation center point as the origin, is corrected into corrected polar coordinate data, whose coordinate values ​​are the corrected scanning angle and the corrected detection distance corresponding to the corrected scanning angle on a two-dimensional tunnel central polar coordinate system with the origin being the midpoint of the line segment connecting the first side and the second side of the tunnel on the optical scanning plane. For each of the aforementioned scanning angles, interpolated polar coordinate data is calculated by interpolation from at least two of the aforementioned corrected polar coordinate data, each having at least two of the aforementioned corrected polar coordinate data that are closest to the aforementioned scanning angle, with the scanning angle and the value of the interpolated detection distance corresponding to the scanning angle as coordinate values, and the arch shape is detected as a set of interpolated polar coordinate data for each of the aforementioned scanning angles calculated by the interpolation operation. The direction angle calculation unit, during autonomous driving of the heavy machinery and equipment, performs the determination for each scanning angle by comparing each set of reference distance values ​​of the reference arch shape stored in advance with the set of interpolated detection distance values ​​included in the set of interpolated polar coordinate data calculated by the arch shape detection unit through the interpolation calculation. Direction angle detection device for autonomous heavy machinery / devices used in tunnels, as described in claim 1.

3. The direction angle calculation unit, in the determination, calculates for each scan angle the difference between the coordinate value of the interpolated detection distance of the interpolated polar coordinate data which includes the scan angle calculated by the interpolation calculation in the arch shape detection unit as a coordinate value and the value of the reference distance corresponding to the scan angle that indicates one of the reference arch shapes, calculates the sum of the squares of the differences calculated over a plurality of scan angles for the one reference arch shape, and determines the reference arch shape with the smallest sum of the squares calculated for a plurality of reference arch shapes stored in advance, the direction angle detection device for autonomous heavy machinery and equipment in a tunnel according to claim 2.

4. The direction angle calculation unit further calculates, in the determination, for each scan angle, the difference between the coordinate value of the interpolated detection distance of the interpolated polar coordinate data which includes the scan angle calculated by the interpolation calculation as a coordinate value and the value of the reference distance corresponding to the scan angle that indicates one of the reference arch shapes, calculates the number of scan angles corresponding to the difference calculated over a plurality of scan angles for which the value is less than or equal to a predetermined threshold as the degree of match, and determines the reference arch shape corresponding to the one with the largest degree of match among the plurality of reference arch shapes stored in advance, as described in claim 2.

5. In a processor for a direction angle detection device that detects the direction angle indicating the direction of travel of heavy machinery and equipment autonomously moving inside a tunnel, Mounted on the aforementioned heavy machinery / device, and equipped with a function to irradiate a laser beam using LiDAR at an arbitrary scanning angle, the device irradiates the laser beam from the irradiation center point toward the inside of the upper part of the tunnel at each predetermined angle change in the optical scanning plane, which is a virtual plane perpendicular to the direction of travel of the heavy machinery / device and includes the irradiation center point of the laser beam, and measures the behavior of the laser beam until it is reflected back from the inner light irradiation position to detect the distance from the irradiation center point to the light irradiation position as the detection distance, and detects the arch shape indicating the inner circumference line based on the set of values ​​of the detection distance detected for each scanning angle, and Direction angle detection method, which involves detecting and storing a set of reference distance values, which are the distances detected for each scanning angle by the arch shape detection process when the heavy machinery or device is pointed toward each of a plurality of reference direction angles, as reference arch shapes corresponding to the reference direction angles, and when the heavy machinery or device is autonomously traveling, determining which of the plurality of reference arch shapes stored in advance, which is a set of reference distance values ​​across a plurality of scanning angles, is closest to the arch shape detected by the arch shape detection process based on the set of detected distance values ​​across a plurality of scanning angles, and calculating the reference direction angle corresponding to the determined reference arch shape as the direction angle, and executing a direction angle calculation process.

6. The computer in the direction angle detection device, which detects the direction angle indicating the direction of travel of heavy machinery and equipment autonomously moving inside a tunnel, Mounted on the aforementioned heavy machinery / device, and equipped with a function to irradiate a laser beam using LiDAR at an arbitrary scanning angle, the device includes the irradiation center point of the laser beam and is located within a virtual plane perpendicular to the direction of travel of the heavy machinery / device, and for each predetermined angle change in the scanning angle, from the irradiation angle of the laser beam directed toward the first side of the tunnel to the irradiation angle of the laser beam directed toward the second side opposite to the first side, the device irradiates the laser beam from the irradiation center point toward the inside of the upper part of the tunnel at the scanning angle, and measures the behavior of the laser beam until it is reflected back from the inner light irradiation position, thereby detecting the distance from the irradiation center point to the light irradiation position as the detection distance, and detecting the arch shape indicating the inner circumference line based on the set of the detection distance values ​​detected for each scanning angle, and A program for executing a direction angle calculation process which involves first detecting and storing a set of reference distance values, which are the distances detected for each scanning angle by the arch shape detection process when the heavy machinery or device is pointed towards each of a plurality of reference direction angles, as reference arch shapes corresponding to the reference direction angles; and during autonomous driving of the heavy machinery or device, determining which of the plurality of reference arch shapes stored in advance, which is the set of reference distance values ​​across the plurality of scanning angles, is closest to the arch shape detected by the arch shape detection process based on the set of detected distance values ​​across the plurality of scanning angles, and calculating the reference direction angle corresponding to the determined reference arch shape as the direction angle.

Images