JP5392700B2 - Obstacle detection device and obstacle detection method - Google Patents

Description

  The present invention relates to an obstacle detection device provided in a moving body such as an automatic guided vehicle, and an obstacle detection method using the obstacle detection device.

2. Description of the Related Art Conventionally, an obstacle detection device that detects an obstacle present on the course of a vehicle is known. For example, in the obstacle detection device described in Patent Document 1, a setting area (for example, outside a guardrail or a curve) where an obstacle is not detected is calculated based on the distance from the vehicle to an installation (guardrail) on the roadside. By excluding the set area from the obstacle detection area, it is possible to prevent erroneous detection in which a non-obstacle is detected as an obstacle.
Japanese Patent Laid-Open No. 5-205198

  By the way, in the tunnel, there are a plurality of underground facilities such as underground telephones and fire extinguishing facilities, and in particular, construction materials and devices are arranged in the tunnel in the middle of construction. In this case, if the obstacle detection device is applied to an unmanned transport vehicle (moving body) used for transporting materials, the above-mentioned equipment, which is originally a non-obstacle, is detected as an obstacle. ) There is a risk of detection. In addition, in the tunnel, it is difficult to accurately detect the distance from the transport vehicle to the inner wall of the tunnel (installed by the road) using the above-mentioned equipment, etc., so the above-described conventional obstacle detection device can accurately detect the obstacle. It is practically difficult to detect an object.

  The present invention aims to solve the above problems, and provides an obstacle detection device and an obstacle detection method capable of preventing erroneous detection of an obstacle and improving the detection accuracy of the obstacle.

  The present invention relates to an obstacle detection apparatus for detecting an obstacle present on a path of a moving body, position / orientation information calculation means for calculating position / orientation information of the moving body, and a surrounding environment of the moving body which is mounted on the moving body. Laser scanner for acquiring shape information, detection area information storage means for storing detection area information along the traveling path of the moving body related to the area where the obstacle should be detected, and position and orientation determined by the position and orientation information calculation means Based on the information and the detection area information stored in the detection area information storage means, the detection target area at the current position of the moving body is specified, and the detection target area and the shape information acquired by the laser scanner overlap. In the detection area calculation means for determining the area as an obstacle detection area for detecting an obstacle, and in the obstacle detection area calculated by the detection area calculation means Characterized in that it comprises a obstacle detecting means for detecting an obstacle from the obtained shape information by a laser scanner.

  In this obstacle detection device, a detection target area is specified based on the position and orientation information of the moving body and the detection area information where the obstacle should be detected, and the detection target area overlaps with the shape information related to the surrounding environment of the moving body. The area to be detected is determined as an obstacle detection area. Then, the obstacle is detected in the obstacle detection area. For example, even if the underground facility is arranged along the traveling path of the mobile body in the tunnel, if the underground facility is not disposed on the course of the mobile body, the underground facility is stored in the detection area information storage means. Detection area information excluded from areas to be detected as obstacles is stored. Therefore, by identifying the detection target area at the current position based on the detection area information and the current position and orientation information, and further determining the area where the detection target area and the shape information overlap as the obstacle detection area, It is possible to prevent erroneous detection of an underground facility that is an obstacle as an obstacle. As a result, the obstacle detection accuracy can be improved.

  In addition, map storage means for storing a three-dimensional map including shape information along the longitudinal direction of the travel path is further provided, and the position and orientation information calculation means is acquired by the shape information extracted from the three-dimensional map and the laser scanner. Based on the shape information, it is preferable to determine the position and orientation information of the moving body on the traveling road. In a space shielded from the outside world such as in a tunnel or a specific large facility, it is not possible to expect positioning of a moving body with high accuracy by GPS. On the other hand, the three-dimensional map includes shape information along the traveling road, and describes the arrangement of existing facilities and objects. Therefore, the position and orientation information can be accurately determined by comparing the shape information actually acquired by the laser scanner and the three-dimensional map.

  In addition, the laser scanner preferably includes a first laser scanner that acquires shape information for determining position and orientation information of the moving body, and a second laser scanner that acquires shape information related to the obstacle. . In this way, each shape information can be acquired more accurately, so that the obstacle detection area can be determined more accurately and the obstacle can be detected more accurately.

  Further, the apparatus further comprises a moving distance detecting means for detecting a moving distance from the starting position of the moving body, and a position / attitude information storing means for storing position / attitude information of the moving body corresponding to the moving distance, Preferably, the position / orientation information of the moving body on the travel path is determined based on the movement distance from the calculated position detected by the movement distance detecting unit and the position / orientation information stored in the position / orientation information storage unit. is there. In a space shielded from the outside world such as in a tunnel or a specific large facility, it is not possible to expect positioning of a moving body with high accuracy by GPS. On the other hand, the position / orientation information storage means stores position / orientation information corresponding to the moving distance of the moving body. Therefore, it is possible to accurately determine the position and orientation information of the moving body by detecting the moving distance of the moving body.

  In addition, a plurality of markers arranged at predetermined positions on the road so as to be distinguishable from the surrounding environment of the road, marker detection means for detecting the marker, and a movement distance detection means when the marker is detected by the marker detection means Correction means for correcting the movement distance based on the marker, and the position / orientation information calculation means includes the movement distance of the movement distance detection means corrected by the correction means and the position / orientation information stored in the position / orientation information storage means. Based on the information, it is preferable to determine the position and orientation information of the moving body on the traveling road. In this way, the movement distance can be corrected each time a marker is detected, and the accumulated error of the movement distance can be corrected. Thereby, since position and orientation information can be determined more accurately, a highly reliable obstacle detection region can be determined.

  Further, it is preferable that the detection area calculation means determines the obstacle detection area based on the traveling state of the moving body. In this way, for example, if the speed of the moving body is high, the obstacle detection area in the longitudinal direction of the travel path can be wide, and conversely if it is slow, it can be narrow. Thereby, an obstacle can be detected suitably.

  The present invention relates to an obstacle detection method for detecting an obstacle present on the path of a moving body, a position and orientation information calculation step for determining position and orientation information of the moving body, and a surrounding environment of the moving body by scanning a laser scanner. Based on the scanning step for acquiring the shape information, the position and orientation information determined in the position and orientation information calculation step, and the detection area information along the traveling path of the moving body regarding the area where the obstacle should be detected, A detection region calculation step for identifying a detection target region at the current position and calculating a region where the detection target region overlaps with shape information acquired by the laser scanner as an obstacle detection region for detecting an obstacle, and detection region calculation In the obstacle detection area determined in the step, an obstacle for detecting the obstacle from the shape information acquired by the laser scanner Characterized in that it comprises a goods detection step.

  According to this obstacle detection method, it is possible to prevent erroneous detection of underground facilities or the like that are non-obstacles as obstacles. As a result, the obstacle detection accuracy can be improved.

  According to the present invention, it is possible to prevent erroneous detection of an obstacle and improve the detection accuracy of the obstacle.

    DESCRIPTION OF EXEMPLARY EMBODIMENTS Hereinafter, preferred embodiments of a moving body guidance system and a guidance method according to the present invention will be described with reference to the drawings.

[First Embodiment]
FIG. 1 is a diagram schematically illustrating a transport vehicle that travels in a tunnel such as a tunnel. FIG. 2 is a perspective view of the transport vehicle. As shown in FIG. 1 or FIG. 2, along the predetermined traveling route R defined in the tunnel T, in order to quickly carry and inspect and perform other operations such as the segment Se of the inner wall Ta accompanying tunnel construction. It is necessary to automatically operate the transport vehicle 3A.

  Here, a plurality of underground facilities such as underground telephones and fire extinguishing facilities, and construction materials and devices used for tunnel construction are arranged in the tunnel T. For this reason, in order to automatically operate the transport vehicle 3A, it is necessary to provide an obstacle detection device in order to detect an obstacle. However, if the transport vehicle 3A stops (slows down) every time an underground facility is detected, the operation is slow. Efficiency is reduced. The guidance system 1A according to the present embodiment has an obstacle detection device that accurately detects other obstacles, except for underground facilities that are known to be arranged in advance in the tunnel T, and the like. This is a system that realizes autonomous operation of the transport vehicle 3A by guiding the transport vehicle 3A by absolute guidance along the travel route R in the tunnel T. The transport vehicle 3A corresponds to a moving body.

  As shown in FIG. 3 or FIG. 4, the transport vehicle 3 </ b> A has a vehicle body portion 5 provided with rolling wheels 5 a, a loading platform 7 provided on the upper portion of the vehicle body portion 5, and a front portion of the vehicle body portion 5. An obstacle sensor / bumper switch 9 provided, a steering device 11 for steering the steering wheel 5a, driving and stopping the wheel 5a (see FIG. 4), and a battery (not shown) are provided. . A load such as a segment Se is loaded on the loading platform 7, and the rotation of the wheels 5 a and the change of the steering angle are executed by the drive device 11.

  In addition, the transport vehicle 3A is input from the first laser scanner 13 attached to the front portion in the traveling direction Dm, the inner world sensor 14 attached to the wheel 5a, the first laser scanner 13, and the inner world sensor 14. And a travel control device 15 that controls the travel of the transport vehicle 3A based on the data. Further, the transport vehicle 3A includes a second laser scanner 16 attached to the front and rear portions of the transport vehicle 3A, and an obstacle in front of the traveling direction Dm of the transport vehicle 3A based on data input from the second laser scanner 16. And an obstacle detection device 17A for detecting.

  The first laser scanner 13 irradiates a laser beam so as to draw a circular orbit toward the front side in the traveling direction Dm of the transport vehicle 3A, and reflects the laser beam returned to the inner wall Ta of the tunnel T (hereinafter referred to as “ A sensor unit that receives the reflected light). The first laser scanner 13 measures the distance to the positioning object from the round-trip time from the irradiation of the laser beam to the reception of the reflected light, and further determines the positioning object from the distance and the irradiation direction of the laser beam. The coordinate data is acquired and input to the travel control device 15. This coordinate data is data as a cross-sectional shape that traverses the tunnel T, and corresponds to shape information for determining position and orientation information of the transport vehicle 3A. Hereinafter, the data as the cross-sectional shape is referred to as observation shape data.

  The second laser scanner 16 irradiates a laser beam so as to draw a circular orbit toward the front side in the traveling direction Dm of the transport vehicle 3A, and reflects it to obstacles (including underground facilities) existing in the tunnel T. And a sensor unit that receives the laser beam returned (hereinafter referred to as “reflected light”). The second laser scanner 16 measures the distance to the positioning object from the round-trip time from irradiation of the laser beam to reception of the reflected light, and further determines the positioning object from the distance and the irradiation direction of the laser beam. The coordinate data is acquired and input to the obstacle detection device 17A. This coordinate data corresponds to shape information regarding the obstacle. Hereinafter, this data is referred to as obstacle data.

  The inner sensor 14 is a sensor that detects a movement distance from the number of rotations of the wheel 5a, and inputs data relating to the detected movement distance to the travel control device 15.

  The travel control device 15 includes a control board on which a CPU, a RAM, a ROM, and the like are mounted, an input / output device, an external storage device, and the like. The travel control device 15 reads predetermined software on hardware such as a CPU and a RAM, whereby an input / output device and the like operate under the control of the CPU, thereby realizing a predetermined function. First, functions executed by the travel control device 15 will be described.

  As shown in FIG. 4, the travel control device 15 functions as a map storage unit (map storage unit) 15a, a position / orientation information calculation unit (position / orientation information calculation unit) 15b, and an absolute guidance control unit 15c.

  The map storage unit 15a stores a three-dimensional map including shape data (shape information) along the travel route R. The shape data along the travel route R is data as a cross-sectional shape that crosses the tunnel T.

  The position / orientation information calculation unit 15b reads the three-dimensional map stored in the map storage unit 15a, and based on the three-dimensional map and the observation shape data acquired by the first laser scanner 13, the position / orientation of the transport vehicle 3A. Determine information. The position / orientation information of the transport vehicle 3A is position information (lateral direction and longitudinal direction) of the transport vehicle 3A on the travel route R and posture angle information (pitch, yaw, roll) of the transport vehicle 3A. As for the position in the longitudinal direction of the transport vehicle 3A in the tunnel T, the position in the longitudinal direction calculated based on the movement distance from the starting position of the transport vehicle 3A detected by the inner sensor 14 can also be adopted. .

  Specifically, the position / orientation information calculation unit 15b estimates the position and orientation of the transport vehicle 3A using a Markov position estimation method based on the three-dimensional map and the observed shape data. At this time, when a straight line with the same cross section or a curved section with a constant curvature of the tunnel T continues with the same cross section, the position in the longitudinal direction of the transport vehicle 3A cannot be uniquely specified only by the surrounding shape features. The position in the longitudinal direction calculated based on the movement distance from the starting position of the transport vehicle 3A detected by the inner sensor 14 can also be adopted.

  The Markov position estimation method will be briefly described below. In the actual method, three dimensions and six degrees of freedom are used. However, in the following description, an example of 1.5 dimensions and one degree of freedom will be described for convenience. FIG. 5 is a diagram showing the positional relationship and the existence probability between three structures installed in the tunnel T and the transport vehicle 3A in order to explain the flow of the Markov position estimation method. ) Are lined up in time series. FIG. 5A shows an initial state. In the traveling route R, three structures Ar are installed in order from the left along the traveling direction of the transport vehicle 3A, and each structure Ar has the same shape. The transport vehicle 3A in the initial state has not yet captured the structure Ar. FIG. 5B is a diagram illustrating the initial sensing time when the transport vehicle 3A captures the first structure Ar, and FIG. 5C illustrates the movement state after the transport vehicle 3A passes through the first structure Ar. It is shown, and it is a figure which shows the state which has not captured the next structure Ar yet. FIG. 5D is a diagram showing a state in which the transport vehicle 3A captures the next structure Ar and the position is specified.

  As shown in FIG. 5A, in the initial state, the existence probability is uniform over the entire range of the travel route R. Assume that the transport vehicle 3A starts moving, and the first laser scanner 13 captures any one of the structures Ar (see FIG. 5B). Since the three structures Ar arranged on the travel route R have the same shape, the position information of the transport vehicle 3A cannot be specified even with reference to the three-dimensional map. Therefore, the position / orientation information calculation unit 15b allocates the existence probability estimated from the observation shape data acquired by the first laser scanner 13 to the vicinity of each structure Ar. For example, the presence predicted value in the vicinity of each structure Ar is set to “1/3”, and the presence predicted value in a range other than the vicinity is set to be lower than 1/3. The existence probability at each point is given as a value obtained by dividing the existence prediction value at that point by the sum of the existence prediction values.

  The transport vehicle 3A continues to move through the first structure Ar (see FIG. 5C). The position / orientation information calculation unit 15b calculates the first existence probability from the past to the present in consideration of the error of the internal sensor 14, and the second existence probability estimated from the first laser scanner 13 at this time. Further, a third existence probability obtained by combining the first existence probability and the second existence probability is determined.

  As shown in FIG. 5D, when the transport vehicle 3A captures the next structure Ar, the position / orientation information calculation unit 15b calculates the second existence probability based on the newly acquired observation shape data. . Then, the position / orientation information calculation unit 15b combines the first existence probability and the second existence probability to determine the third existence probability. Here, only the observation shape data of the first structure Ar could not identify the protruding portion (position) in the third existence probability, but if the observation shape data of the next structure Ar can be captured, the third A protruding portion (position) occurs in the existence probability. The position / orientation information calculation unit 15b estimates the position having the highest third existence probability as the position in the lateral direction and the longitudinal direction of the transport vehicle 3A.

  The position / orientation information calculation unit 15b discretizes the Markov position estimation method and estimates the position and orientation using a PF (particle filter) method that is realized numerically. Theoretically, the correct position can be estimated as the transport vehicle 3A moves even if the initial value is not given. However, since the correct position and posture are always necessary for the operation of the transport vehicle 3A, the initial position of the transport vehicle 3A is required. Give position and posture.

  The position / orientation information calculation unit 15b includes a position in the longitudinal direction calculated based on the moving distance from the starting position of the transport vehicle 3A detected by the inner sensor 14, observation shape data obtained from the first laser scanner 13, Then, shape data expected to be obtained from the first laser scanner 13 is calculated by a three-dimensional coordinate calculation from the three-dimensional map in the map storage unit 15a. The position and posture angle for performing the estimation are given by an initial value or a previous estimated value and a combination that can occur from the movement of the transport vehicle 3A during the measurement interval of the internal sensor 14 or the first laser scanner 13. . Then, the position / orientation information calculation unit 15b performs matching between a large number of shape data obtained from the respective positions and posture angles and the observed shape data, and obtains the position and posture angle corresponding to the shape data having the highest degree of coincidence. It specifies that it is the position and attitude | position angle of 3 A of conveyance vehicles at the present time. As described above, the position / orientation information calculation unit 15b determines the position / orientation information of the transport vehicle 3A.

  As shown in FIG. 4, the absolute guidance control unit 15 c controls traveling of the transport vehicle 3 </ b> A by absolute guidance based on the position and orientation information determined by the position and orientation information calculation unit 15 b.

  Absolute induction is a concept contrasted with relative induction. Relative guidance is intended to be guided based on position and orientation information (relative position and posture angle) determined from the relative positional relationship with the inner wall Ta of the tunnel T and other structures. The transport vehicle 3A is guided so as to travel at a certain distance from the structure. On the other hand, the absolute guidance is intended to guide the transport vehicle 3A based on position information and posture angle information determined as absolute information in the tunnel T.

  The obstacle detection device 17A includes a control board on which a CPU, a RAM, a ROM, and the like are mounted, an input / output device, an external storage device, and the like. The obstacle detection device 17A implements a predetermined function by causing the input / output device or the like to operate under the control of the CPU by reading predetermined software on hardware such as the CPU and RAM. Functions executed by the obstacle detection device 17A will be described.

  As shown in FIG. 4, the obstacle detection device 17A includes a detection area information storage unit (detection area information storage unit) 17a, a detection area calculation unit (detection area calculation unit) 17b, and an obstacle detection unit 17c (obstacle detection). Means), functioning as the control unit 17d.

  The detection area information storage unit 17a stores detection area information along the travel route (travel path) R of the transport vehicle 3A related to an area where an obstacle should be detected. The detection area information along the travel route R is an arrangement position (part) of the underground facilities, construction materials, equipment used for construction, etc., which are previously arranged in the tunnel T so as not to obstruct the travel of the transport vehicle 3A. Is information indicating a region portion excluded from the region where an obstacle should be detected in the travel route R. The detection region information may be a two-dimensional region or a region as a three-dimensional space.

  The detection area calculation unit 17b reads the detection area information stored in the detection area information storage unit 17a based on the position and orientation information of the transport vehicle 3A, and the obstacle acquired by the detection area information and the second laser scanner 16 Based on the data, an obstacle detection area for detecting an obstacle is determined. Specifically, this will be described with reference to FIG. The detection area C1 of the second laser scanner 16 is in the range shown in FIG. Therefore, when the underground facility K exists in the detection area C1, the underground facility K is normally detected as an obstacle. Therefore, the detection area calculation unit 17b reads the obstacle detection area from the detection area information storage unit 17a based on the position and orientation information of the transport vehicle 3A, and the current position of the transport vehicle 3A as shown in FIG. The detection target area C2 is identified.

  Then, as shown in FIG. 6C, the detection area calculation unit 17b detects an obstacle in an area where the detection target area C2 and the detection area C1 of the second laser scanner 16 overlap. The area C3 is determined. Thereby, the underground facility K is excluded from the obstacle detection area C3 and exists in the non-detection area C4 of the obstacle.

  Further, the detection area calculation unit 17b changes the obstacle detection area C3 based on the vehicle speed (traveling state) of the transport vehicle 3A. Specifically, the detection area calculation unit 17b expands the obstacle detection area C3 in the longitudinal direction of the tunnel T or the detection area C1 of the second laser scanner 16 when the speed of the transport vehicle 3A is high. Is expanded only in the forward direction of the vehicle. When the speed of the transport vehicle 3B is low, the obstacle detection area C3 is reduced in the longitudinal direction of the tunnel T, or the detection area C1 of the second laser scanner 16 is reduced only in the forward direction of the vehicle. In this way, when the speed of the transport vehicle 3A is high, if the obstacle detection area C3 is enlarged as compared with the case where the vehicle speed is low, there is a time lag between the detection of the obstacle and the stop of the transport vehicle 3A. Even if there is, the transport vehicle 3A can be stopped reliably. The obstacle detection area C3 may be changed according to the traveling mode of the transport vehicle 3A or a control signal transmitted from the outside.

  The obstacle detection unit 17c detects an obstacle in the tunnel T based on the obstacle detection region C3 determined by the detection region calculation unit 17b. When the obstacle detection unit 17c detects an obstacle present in the detection area C1 of the second laser scanner 16 in the obstacle detection area C3, the obstacle detection flag is set.

  When the obstacle detection flag is set by the obstacle detection unit 17c, the control unit 17d outputs a control signal that causes the driving device 11 to stop, slow down, or avoid the conveyance vehicle 3A.

  Next, a guidance method and an obstacle detection method using the guidance system 1 will be described with reference to FIGS. FIG. 7 is a flowchart showing the operation procedure of the guidance method and obstacle detection method according to the present embodiment, and FIG. 8 is a flowchart showing the operation procedure of the obstacle detection process.

  As shown in FIG. 7, when guidance of the transport vehicle 3A is started, the position / orientation information calculation unit 15b performs initial setting indicating that the position of the transport vehicle 3A is at the start position, and the drive device 11 performs the transport vehicle 3A. Starts moving (step S01). Next, the first laser scanner 13 performs a scanning process to acquire observation shape data (step S02). Step S02 corresponds to a scanning step.

  Subsequently, the position / orientation information calculation unit 15b executes position / orientation estimation calculation by a Markov position estimation method based on the observed shape data (step S03). Step S03 corresponds to a position / orientation information calculation step. Then, the absolute guidance control unit 17c performs absolute guidance control of the transport vehicle 3A based on the determined position and orientation information (step S04). Also, the obstacle detection device 17A performs an obstacle detection process based on the determined position and orientation information (step S05). The autonomous operation of the transport vehicle 3A is continued while performing the above processing, and when reaching the stop position, the guidance control process is terminated, the movement of the transport vehicle 3A is stopped (step S06), and the guidance of the transport vehicle 3A is terminated. To do.

  Further, the operation procedure of the above-described process (step S05) will be described in detail with reference to FIG. When the obstacle detection process is started, the obstacle detection unit 17c resets the obstacle detection flag (step S11). Next, the second laser scanner 16 performs a scanning process to acquire obstacle data (step S12). Step S12 corresponds to a scanning step. Subsequently, the obstacle detection area calculation unit 17b calculates the obstacle detection area C3 based on the obstacle data and the position / orientation information (step S13). Step S13 corresponds to a detection area calculation step.

  Then, the obstacle detection unit 17c determines whether an obstacle is detected in the obstacle detection area C3 (step S14), and sets an obstacle detection flag when an obstacle is detected (step S15). . On the other hand, if no obstacle is detected, the process is terminated. Step S14 corresponds to an obstacle detection step. The processes in steps S11 to S15 are repeated until the transport vehicle 3A stops.

  As described above, according to the obstacle detection device 17A of the guidance system 1A, the detection target region C2 is specified based on the position and orientation information of the transport vehicle 3A and the detection region information on which the obstacle should be detected, and the detection target region C2 and the obstacle An area where data overlaps is determined as an obstacle detection area C3. Then, the obstacle is detected in the obstacle detection area C3. For example, even if the underground facility K is arranged along the travel route R of the transport vehicle 3A in the tunnel, if the underground facility K is not disposed on the course of the transport vehicle 3A, the detection area information storage unit The detection area | region information excluded from the area | region which the mine equipment K should detect as an obstruction is memorize | stored in 17a. Therefore, the detection target area C2 at the current position is specified based on the detection area information and the current position and orientation information, and an area where the detection target area C2 and the obstacle data overlap is further determined as the obstacle detection area C3. This prevents erroneous detection of a mine facility K or the like that is a non-obstacle as an obstacle. As a result, the obstacle detection accuracy can be improved.

  In addition, a map storage unit 15a that stores a three-dimensional map including shape information along the longitudinal direction of the travel route S is further provided, and the position / orientation information calculation unit 15b includes the shape information extracted from the three-dimensional map and the first information. Based on the observation shape data acquired by the 1 laser scanner 13, the position and orientation information of the transport vehicle 3A on the travel route S is determined. In a space shielded from the outside world such as in a tunnel or a specific large facility, it is not possible to expect positioning of a moving body with high accuracy by GPS. On the other hand, the three-dimensional map includes shape information along the travel route R, and describes the arrangement of existing facilities and objects. Therefore, the position and orientation information can be accurately determined by comparing the observation shape data actually acquired by the first laser scanner 13 with the three-dimensional map.

  Moreover, since the first laser scanner 13 that mainly obtains observation shape data for determining the position and orientation information of the transport vehicle 3A and the second laser scanner 16 that mainly obtains obstacle data are provided, the observation shape data and Obstacle data can be obtained accurately, the obstacle detection area C3 can be determined more accurately, and obstacles can be detected more accurately.

[Second Embodiment]
Next, with reference to FIG.9 and FIG.10, the guidance system which concerns on 2nd Embodiment of this invention is demonstrated. In addition, about the guidance system 1B which concerns on this embodiment, about the element similar to the guidance system 1A which concerns on 1st Embodiment, the code | symbol similar to 1st Embodiment is attached | subjected and detailed description is abbreviate | omitted.

  As shown in FIG. 9 and FIG. 10, the guidance system 1B includes a laser scanner 18 (second laser scanner), an internal sensor 14, and the like mounted on the transport vehicle 3B, like the transport vehicle 3A of the first embodiment. A travel control device 19 and an obstacle detection device 17B are provided, and a marker sensor (marker detection means) 20 is further provided. The transport vehicle 3B travels on the rail L arranged along the travel route S.

  A plurality of (four in this embodiment) marker sensors 20 are arranged in parallel at the bottom of the vehicle body 5 of the transport vehicle 3B. The marker sensor 20 is, for example, a photoelectric switch that detects the markers (reflectors) M1 to M4 disposed on the inner side of the rail L. The marker M1 projects light on the markers M1 to M4 and receives the reflected light to thereby detect the marker M1. ˜M4 is detected.

  As shown in FIG. 11, the markers M <b> 1 to M <b> 4 are positions of the marker sensors 20 arranged in parallel at the bottom of the main body 5 of the transport vehicle 3 at a constant interval (for example, 100 m) along the rail L. Are arranged along the rail L so as to correspond. The markers M1 to M4 can be distinguished from the surrounding environment.

  The travel control device 19 functions as a position and orientation information storage unit (position and orientation information storage unit) 19a, a correction unit (correction unit) 19b, a marker data storage unit 19c, a position and orientation information calculation unit 19d, and a travel control unit 19e.

  The position / orientation information storage unit 19a stores a table in which the movement distance of the transport vehicle 3B in the mine tunnel T detected by the inner sensor 14 is associated with the position / orientation information of the transport vehicle 3B at the movement distance. . The position and orientation information can be read by obtaining the position and orientation information of the transport vehicle 3B.

  When the marker sensor 20 detects the markers M1 to M4, the correction unit 19b corrects the movement distance detected by the inner world sensor 14 based on the markers M1 to M4. Specifically, when the markers M1 to M4 are detected, the correction unit 19b refers to a table stored in the marker data storage unit 19c, and based on the movement distance indicated by the markers M1 to M4, The movement distance of the field sensor 14 is corrected.

  FIGS. 12A and 12B are diagrams illustrating tables stored in the marker data storage unit 19c. As shown in FIGS. 12A and 12B, the tables TB1 and TB2 store each marker sensor 20 and the moving distance of the transport vehicle 3B corresponding to the count number of the marker sensor 20 in association with each other. ing. Specifically, in TB1, for example, when the count number of detection of the marker M2 of the sensor 2 is “2”, data indicating the movement distance of “500 m” is stored. Therefore, when the marker sensor 20 detects the markers M1 to M4, the correction unit 19b refers to the table TB1, reads out the movement distance stored in association with the count number of each marker sensor 20, and the transport vehicle 3B The moving distance (position in the longitudinal direction) in the mine tunnel T. Note that TB1 in FIG. 12 (a) is a table indicating the “going” travel distance to the destination of the transport vehicle 3B, and TB2 in FIG. 12 (b) is “return from the destination of the transport vehicle 3B”. It is a table which shows the movement distance "(to the entrance part of the tunnel T)".

  The position / orientation information calculation unit 19d is a table of the position / orientation information storage unit 19a based on the movement distance of the conveyance vehicle 3B detected by the inner sensor 14 or the movement distance of the conveyance vehicle 3B corrected by the correction unit 19b. The position / orientation information of the transport vehicle 3B on the travel route S is determined. The position and orientation information of the transport vehicle 3B includes the position information (lateral and longitudinal directions) of the transport vehicle 3B on the travel route R (rail L) and the posture angle information (pitch, yaw, Roll).

  The traveling control unit 19e controls traveling to the destination of the transport vehicle 3B based on the position and orientation information of the transport vehicle 3B. The traveling control unit 19e controls traveling of the transport vehicle 3B by changing the vehicle speed of the transport vehicle 3B according to a preset section, a section with a sharp curve of the rail L, or a signal from a sensor or the outside.

  The obstacle detection device 17B includes a detection area information storage unit 17a, an obstacle detection region calculation unit 17b, an obstacle detection unit 17c, and a control unit 17d, similarly to the obstacle detection device 17A according to the first embodiment. . The obstacle detection device 17B performs the same process as the obstacle detection device 17A according to the first embodiment to detect an obstacle.

  Next, a guidance method and an obstacle detection method using the guidance system 1B will be described with reference to FIGS. FIG. 13 is a flowchart showing the operation procedure of the guidance method and the obstacle detection method according to this embodiment, and FIG. 14 is a flowchart showing the operation procedure of the obstacle detection process.

  As illustrated in FIG. 13, when the guidance of the transport vehicle 3B is started, the position / orientation information calculation unit 19d performs initial setting indicating that the position of the transport vehicle 3B is at the start position, and the driving device 11 performs the transport vehicle 3B. Is started (step S21). Next, the movement distance in the tunnel T of the transport vehicle 3B is detected by the inner sensor 14 (step S22). Step S22 corresponds to a scanning step.

  Subsequently, the correcting unit 19b determines whether or not the marker sensor 20 has detected the markers M1 to M4 (step S23). When the markers M1 to M4 are detected, the correction unit 19b refers to the marker data storage unit 19c and corrects the movement distance of the inner sensor 14 based on the count number of each marker sensor 20. (Step S24). On the other hand, when the markers M1 to M4 are not detected, the process proceeds to step S25.

  In step S25, the position / orientation information calculation unit 19d refers to the position / orientation information storage unit 19a based on the movement distance, and executes position / orientation estimation calculation (step S26). Step S26 corresponds to a position / orientation information calculation step. Then, the obstacle detection device 17B performs an obstacle detection process based on the determined position and orientation information (step S27). The autonomous operation of the transport vehicle 3B is continued while performing the above processing, and when reaching the stop position, the guidance control processing is terminated, the movement of the transport vehicle 3B is stopped (step S28), and the guidance of the transport vehicle 3B is terminated. To do.

  Further, the operation procedure of the above-described process (step S27) will be described in detail with reference to FIG. When the obstacle detection process is started, the obstacle detection unit 17c resets the obstacle detection flag (step S31). Next, the second laser scanner 16 performs a scanning process to acquire obstacle data (step S32). Step S32 corresponds to a scanning step. Subsequently, the obstacle detection area calculation unit 17b calculates the obstacle detection area C3 based on the obstacle data and the position and orientation information (step S33). Step S33 corresponds to a detection area calculation step.

  Then, the obstacle detection unit 17d determines whether or not an obstacle is detected in the obstacle detection area C3 (step S34), and sets an obstacle detection flag when an obstacle is detected (step S35). . On the other hand, if no obstacle is detected, the process is terminated. Step S34 corresponds to an obstacle detection step. The processes of steps S31 to S35 are repeated until the transport vehicle 3B stops.

  As described above, according to the obstacle detection device 17B of the guidance system 1B, it is possible to improve the detection accuracy of the obstacle, similarly to the obstacle detection device 17A according to the first embodiment. In addition, a position / orientation information storage unit 19a that stores position / orientation information of the transport vehicle 3B corresponding to the movement distance is provided, and the position / orientation information calculation unit 19d is a movement distance from the calculated position detected by the inner sensor 14. And the position and orientation information of the transport vehicle 3B on the travel route S are determined based on the position and orientation information stored in the position and orientation information storage unit 15a. In a space shielded from the outside world such as in a tunnel or a specific large facility, it is not possible to expect positioning of a moving body with high accuracy by GPS. On the other hand, the position / orientation information storage unit 19 a stores position / orientation information corresponding to the moving distance of the transport vehicle 3. Therefore, the position and orientation information of the transport vehicle 3B can be accurately determined by detecting the moving distance of the transport vehicle 3B.

  When a plurality of markers M1 to M4 are arranged at predetermined positions on the travel route S (rail L) so as to be distinguishable from the surrounding environment of the travel route R, and the marker sensor 20 detects the markers M1 to M4. In addition, the correction unit 19b corrects the movement distance in the inner world sensor 14 based on the markers M1 to M4. For this reason, the position / orientation information calculation unit 19d performs conveyance on the travel route R based on the movement distance of the inner sensor 14 corrected by the correction unit 19b and the position / orientation information stored in the position / orientation information storage unit 19a. The position and orientation information of the vehicle 3B is determined. In this way, the movement distance can be corrected each time the markers M1 to M4 are detected, and the accumulated error of the movement distance can be corrected. Thereby, since position and orientation information can be determined more accurately, a highly reliable obstacle detection area can be determined.

  As mentioned above, although this invention was demonstrated based on each embodiment, this invention is not limited only to these embodiment. For example, in the first embodiment, the first laser scanner 13 and the second laser scanner 16 are provided, but the observation shape data and the obstacle data may be acquired by one laser scanner.

  In the second embodiment, a table indicating the movement distance corresponding to the count number of each marker sensor 20 is stored in the marker data storage unit 19c, and the correction unit 19b refers to the table to determine the movement distance. Although corrected, the movement distance may be calculated and corrected from the count number of the marker sensor 20 and the arrangement intervals in the longitudinal direction of the markers M1 to M4. Specifically, when the arrangement interval of the markers M1 to M4 is 100 m and the count number is “3”, for example, the correction unit 19b calculates 3 × 100 m = 300 m to calculate the movement distance. It may be corrected.

It is a figure which shows typically the conveyance vehicle which drive | works a tunnel, such as a tunnel. It is a perspective view of the conveyance vehicle which concerns on this embodiment. It is a side view of the conveyance vehicle which concerns on this embodiment. It is a block diagram of the guidance system concerning this embodiment. In order to explain the flow of the Markov position estimation method, it is a diagram showing the positional relationship and the existence probability of three structures arranged in a tunnel and a transport vehicle. It is a figure which shows the detection area of a 2nd laser scanner, and the area | region in the detection area information memorize | stored in the detection area information storage part in order to demonstrate calculation of an obstacle detection area. It is a flowchart which shows the operation | movement procedure of the process of the guidance method and obstacle detection method which concern on this embodiment. It is a flowchart which shows the operation | movement procedure of an obstruction detection process. It is a side view of the conveyance vehicle which concerns on 2nd Embodiment of this invention. It is a block diagram of the conveyance vehicle which concerns on 2nd Embodiment of this invention. It is a figure which shows arrangement | positioning of a marker. It is a figure which illustrates the table memorized by the marker data storage part. It is a flowchart which shows the operation | movement procedure of the process of the guidance method and obstacle detection method which concern on 2nd Embodiment of this invention. It is a flowchart which shows the operation | movement procedure of the obstruction detection process which concerns on 2nd Embodiment of this invention.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1A, 1B ... Guidance system, 3A, 3B ... Conveyance vehicle (moving body), 13, 16, 18 ... Laser scanner (1st laser scanner, 2nd laser scanner), 15a ... Map storage part, 15b, 19d ... Position and orientation Information calculation section (position and orientation information calculation means), 17a ... detection area information storage section (detection area information storage means), 17b ... detection area calculation section (detection area calculation means), 17c ... obstacle detection section (obstacle detection means) ), 19a: Position and orientation information storage unit (position and orientation information storage means), 20: Marker sensor (marker detection means), C2: Detection target area, C3: Obstacle detection area, M1 to M4: Marker, R: Travel route (Runway).

Claims (6)

In the obstacle detection device for detecting an obstacle present on the path of the moving object,
Position and orientation information calculating means for determining the position and orientation information of the moving body;
A laser scanner that is mounted on the moving body and acquires shape information about the surrounding environment of the moving body;
Detection area information storage means for storing detection area information along the traveling path of the moving body related to the area where the obstacle should be detected;
Based on the position / orientation information determined by the position / orientation information calculation means and the detection area information stored in the detection area information storage means, a detection target area at the current position of the moving body is specified. Detecting area calculation means for determining an area where the detection target area and the shape information acquired by the laser scanner overlap as an obstacle detecting area for detecting the obstacle based on the traveling state of the moving body When,
In the obstacle detection area determined by the detection area calculating means, the obstacle detection means for detecting the obstacle from the shape information acquired by the laser scanner ,
The obstacle detection device is characterized in that the detection area calculation means determines the obstacle detection area in which the obstacle detection area is enlarged when the vehicle speed of the moving body is high compared to when the vehicle speed is low . Map storage means for storing a three-dimensional map including shape information along the longitudinal direction of the travel path;
The position / orientation information calculation means calculates position / orientation information of the moving body on the travel path based on shape information extracted from the three-dimensional map and shape information acquired by the laser scanner. The obstacle detection apparatus according to claim 1.   The laser scanner includes a first laser scanner that acquires shape information for determining position and orientation information of the moving body, and a second laser scanner that acquires shape information related to the obstacle. The obstacle detection device according to claim 2. A moving distance detecting means for detecting a moving distance from the starting position of the moving body;
A position / orientation information storage unit that stores position / orientation information of the moving body corresponding to the moving distance;
The position / orientation information calculation means is configured to detect the moving object on the travel path based on the movement distance detected by the movement distance detection means and the position / orientation information stored in the position / orientation information storage means. The obstacle detection apparatus according to claim 1, wherein position and orientation information is determined. A plurality of markers arranged at a predetermined position on the travel path so as to be distinguishable from the surrounding environment of the travel path,
Marker detection means for detecting the marker;
A correction means for correcting the movement distance in the movement distance detection means based on the marker when the marker is detected by the marker detection means;
The position / orientation information calculation unit is configured to perform the operation on the travel path based on the movement distance of the movement distance detection unit corrected by the correction unit and the position / orientation information stored in the position / orientation information storage unit. 5. The obstacle detection apparatus according to claim 4, wherein the position and orientation information of the moving body is determined. In an obstacle detection method for detecting an obstacle present on the path of a moving object,
A position and orientation information calculation step for determining the position and orientation information of the mobile body;
A scanning step of scanning a laser scanner to obtain shape information related to the surrounding environment of the moving body;
Based on the position / orientation information determined in the position / orientation information calculation step and detection area information along the traveling path of the moving object relating to the area where the obstacle should be detected, the current position of the moving object is determined. An obstacle detection area that identifies a detection target area and detects the obstacle based on a traveling state of the moving object in an area where the detection target area and the shape information acquired by the laser scanner overlap. Detection area calculation step to be determined as
An obstacle detection step of detecting the obstacle from the shape information acquired by the laser scanner in the obstacle detection area determined in the detection area calculation step;
In the detection area calculation step, when the vehicle speed of the moving body is high, the obstacle detection area obtained by enlarging the obstacle detection area compared to when the vehicle speed is low is determined .

Images