WO2018164203A1 - Scanner, working machine, and wheel stopper detecting device - Google Patents

Scanner, working machine, and wheel stopper detecting device Download PDF

Info

Publication number
WO2018164203A1
WO2018164203A1 PCT/JP2018/008859 JP2018008859W WO2018164203A1 WO 2018164203 A1 WO2018164203 A1 WO 2018164203A1 JP 2018008859 W JP2018008859 W JP 2018008859W WO 2018164203 A1 WO2018164203 A1 WO 2018164203A1
Authority
WO
WIPO (PCT)
Prior art keywords
measuring device
distance measuring
distance
scanner
work machine
Prior art date
Application number
PCT/JP2018/008859
Other languages
French (fr)
Japanese (ja)
Inventor
大基 手塚
幸彦 小野
Original Assignee
日立建機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 日立建機株式会社 filed Critical 日立建機株式会社
Publication of WO2018164203A1 publication Critical patent/WO2018164203A1/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60P—VEHICLES ADAPTED FOR LOAD TRANSPORTATION OR TO TRANSPORT, TO CARRY, OR TO COMPRISE SPECIAL LOADS OR OBJECTS
    • B60P1/00—Vehicles predominantly for transporting loads and modified to facilitate loading, consolidating the load, or unloading
    • B60P1/04—Vehicles predominantly for transporting loads and modified to facilitate loading, consolidating the load, or unloading with a tipping movement of load-transporting element
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
    • G01B11/245—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures using a plurality of fixed, simultaneously operating transducers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes

Definitions

  • the present invention relates to a scanner, a work machine, and a vehicle stop detection device for the work machine.
  • Patent Document 1 states that “an optical operation method using at least two intersecting light planes” (summary excerpt) for the purpose of “providing a simple and quick method of level profile measurement for surface roughness inspection of roads and the like” Is disclosed.
  • Patent Document 2 discloses a technique for detecting an obstacle by mounting a plurality of obstacle sensors on a dump truck and varying the scanning range on the road surface of each obstacle sensor.
  • the dump truck for mining When the dump truck for mining releases the load, it may move backward toward the car stop installed at the earth release site and stop immediately before the car stops.
  • This car stop is formed with a ground clearance to function as a dump truck tire stop, but because the car stop is formed with embankment, the shape is different individually, and the shape of the car stop is constant Even if it is formed, the shape may change due to the collapse of the embankment. Therefore, in order to stop the dump truck immediately before the car stop, there is a demand for accurately detecting the position and shape of the car stop.
  • Patent Literature 1 and Patent Literature 2 scan the road surface using a plurality of obstacle sensors, but neither literature considers detecting the height or shape of a car stop having a height relative to the road surface. . Therefore, for example, in Patent Document 1, “irradiation light is irradiated so as to scan an optical plane 10L formed by the intersections 2L, AL, and BL and an optical plane 10R formed by the intersections 2R, AR, and BR. In the case of “reflected along the line 7R to be reflected light” (see paragraph 0008 of Patent Document 1), a blind spot is formed between the line 7L and the line 7R. If there is a car stop here, the distance and shape to the car stop This causes a problem that cannot be detected.
  • the scan surfaces 40a and 40b are shifted along the width direction of the dump truck.
  • both the scanning surfaces 40a and 40b can only acquire distance information on the intersection line between the scanning surface and the road surface from the vicinity of the dump truck to the distance.
  • the shape of the car stop along the vehicle width direction cannot be acquired. Therefore, even if a part of the car stop breaks and the embankment accumulates in front of the dump truck, the height of the deposited soil and the shape of the broken car stop cannot be detected unless the scan surface overlaps the piled earth. This causes a problem such that the mine rides on sedimentary soil. Therefore, there is a situation where it is desired to develop a technique suitable not only for detecting the position of the car stop but also for detecting the car stop shape.
  • the present invention has been made in view of the above circumstances, and an object of the present invention is to provide a technique suitable for detecting a car stop shape installed particularly in a mine.
  • the present invention is a scanner for detecting an obstacle, and the scanner includes a first distance measuring device and a second distance measuring device, and the first distance measuring device and the second distance measuring device.
  • Each of the devices includes a laser range scanner that irradiates a laser, receives reflected light reflected by the detected object, and measures a distance to the detected object, and includes the first distance measuring device and the first distance measuring device.
  • the two-distance measuring device is characterized in that the laser irradiation surface of the first distance measuring device and the laser irradiation surface of the second distance measuring device are arranged orthogonal to each other.
  • FIG. 1 is an explanatory diagram showing the surrounding environment when the dump truck 1 according to the present embodiment is released.
  • FIG. 2 is a left side view of the dump truck 1.
  • FIG. 3 is a rear perspective view of the dump truck 1.
  • the dump truck 1 travels backward on the traveling road surface 1001 toward the car stop 1002 disposed on the cliff side when the dump truck 1 is earthed at the earth ground 1003 under the cliff.
  • the dump truck 1 includes a right front wheel 3r and a left front wheel 3l (see FIG. 2) at a front lower part of a body frame 2 (see FIG. 2), and a right rear wheel 4r and a left rear wheel 4l at a rear lower part.
  • a rear axle 7 is provided at the rear portion of the vehicle body frame 2.
  • a driver's seat 5 (see FIG. 2) is provided at the front upper part of the body frame 2
  • a vessel 6 is provided at the rear upper part.
  • the dump truck 1 is based on a scanner 200 including a plurality of laser scanners, a self-position estimation device 300 that detects the self-position of the dump truck 1, a sensor output from the scanner 200, and a self-position from the self-position estimation device 300. And a reverse support device 100 that supports the reverse travel of the dump truck 1.
  • the position of the dump truck 1 when the backward support device 100 starts operating is the origin of the orthogonal coordinate system 400, and the traveling direction at the moment when the backward support device 100 starts operating is the y axis.
  • the vehicle body height axis of the dump truck 1 is defined as the z axis
  • the axis orthogonal to the two axes of the y axis and the z axis is defined as the x axis.
  • the scanner 200 includes a first distance measuring device 201 and a second distance measuring device 202.
  • the scanner 200 is installed between the left rear wheel 4l and the right rear wheel 4r at the rear of the body of the dump truck 1, more specifically, above the rear axle 7 and below the vessel 6.
  • the first distance measuring device 201 and the second distance measuring device 202 do not need to be arranged at the same height and may not be symmetrical.
  • the first distance measuring device 201 and the second distance measuring device 202 are, for example, a laser scanner (laser) that irradiates a laser beam in a fan shape, receives reflected light from the detected object, and detects the distance and direction to the detected object. Range scanner).
  • the scanner 200 makes the laser irradiation surface (hereinafter referred to as “second scan surface”) 22 of the second distance measurement device 202 orthogonal to the laser irradiation surface (hereinafter referred to as “first scan surface”) 21 of the first distance measurement device 201.
  • the second distance measuring device 202 and the first distance measuring device 201 are arranged.
  • the orthogonality is not limited to the case where the normal vectors of the first scan plane 21 and the second scan plane 22 are strictly perpendicular, but the same detected object as the detected object detected by the first distance measuring device 201. This includes the case where the first distance measuring device 201 and the second distance measuring device 202 are arranged so that the second scan surface 22 faces in a direction in which the shape in the width direction of the body can be detected.
  • the first scanning surface 21 extends at least vertically downward from the vehicle body horizontal plane H including the front and rear axes and the left and right axes of the vehicle body frame 2 to the direction parallel to the vehicle body horizontal plane H toward the rear of the vehicle body.
  • the first distance measuring device 201 is installed on the dump truck 1 so as to cover it.
  • the second distance measuring device 202 uses a position P1 where the second scanning surface 22 and the traveling road surface 1001 intersect, and the rear end of the left rear wheel 4l and the right rear wheel 4r as the traveling road surface 1001.
  • the second distance measuring device 202 is determined by the detection distance of the second distance measuring device 202, the installation depression angle ⁇ of the second distance measuring device 202 with respect to the vehicle body horizontal plane H, and the shielding of the dump truck 1 with respect to the second scan surface 22. It is installed at a position where the length D2 (see FIG. 3) of the intersection line between the road surface 1001 and the second scan surface 22 is longer than the wheel width (outer width) of the dump truck 1. By installing in this way, it is possible to scan in advance the area through which the wheels of the dump truck pass.
  • first distance measuring device 201 and the second distance measuring device 202 are not limited to the configuration provided one by one, and each measuring device may be configured using a plurality of laser scanners. Further, the first scanning surface 21 is not limited to one for the first distance measuring device 201, and a plurality of first scanning surfaces 21 may be provided. Similarly, the second scanning surface 22 is not limited to one for the second distance measuring device 202 and may be plural.
  • FIG. 4 is a block diagram showing a functional configuration of the dump truck 1.
  • the dump truck 1 includes a wheel speed sensor 301 and a yaw rate sensor (turning angular velocity sensor) 302.
  • the self-position estimation apparatus 300 is connected to each of the wheel speed sensor 301 and the yaw rate sensor 302, acquires the wheel rotation speed from the wheel speed sensor 301, and acquires the measurement result of the turning angular velocity of the dump truck 1 from the yaw rate sensor 302.
  • the self-position estimation apparatus 300 calculates the speed and angular velocity of the dump truck 1 and the position and orientation from the origin of the aforementioned orthogonal coordinate system 400 (see FIG. 1) by executing, for example, dead reckoning processing.
  • an IMU inertia measurement device
  • a steering angle sensor may be used instead of the yaw rate sensor 302 for the purpose of estimating the turning angular speed.
  • a GPS Global Positioning System
  • a geomagnetic sensor may be used instead of the self-position estimation apparatus 300.
  • the reverse assistance device 100 includes a vehicle stop detection device 10 (vehicle stop detection controller) and a vehicle stop guidance device 60 (vehicle stop guidance controller).
  • vehicle stop detection device 10 includes a vehicle stop position estimation unit 101 and a vehicle stop width direction shape estimation unit 102.
  • vehicle stop guidance device 60 includes a vehicle stop guidance determination device 61 and a vehicle body shape storage device 62.
  • the car stop position estimation unit 101 includes a road surface calculator 40 and a car stop position calculator 41.
  • the output stage of the first distance measuring device 201 described above is connected to the input stages of the road surface line calculator 40 and the vehicle stop position calculator 41.
  • the output stage of the road surface calculator 40 is connected to a car stop position calculator 41 and a pitch angle corrector 50 described later.
  • the output stage of the first distance measuring device 201 is further connected to the input stage of the car stop position calculator 41.
  • the first distance information R1i output from the first distance measuring device 201 (i: indicates the laser irradiation angle ⁇ in the first distance measuring device 201, for example, ⁇ 45 ⁇ i ⁇ 225, the same applies to the second distance information R2i described later).
  • the measurement point and the distance to the measurement point are included.
  • the road line calculator 40 extracts distance information indicating the measurement point and distance estimated as the road surface from the first distance information R1i, and calculates the road line. Then, the inclination angle ⁇ (see FIG.
  • the inclination angle ⁇ is also referred to as a pitch angle ⁇ .
  • the vehicle stop position calculator 41 extracts the distance information of what is estimated to be a vehicle stop from the first distance information R1i measured by the first distance measuring device 201, the inflection point of the road surface and the vehicle stop, and the inclination of the vehicle stop with respect to the road surface. Is calculated.
  • the car stop width direction shape estimation unit 102 includes a pitch angle corrector 50, a car stop width direction shape calculator 51, and a car stop width direction shape memory 52.
  • the input stage of the pitch angle corrector 50 is connected to the respective output stages of the second distance measuring device 202 and the road surface calculator 40.
  • the output stage of the pitch angle corrector 50 is connected to the input stage of the vehicle stop width direction shape calculator 51.
  • the input stage of the vehicle stop width direction shape calculator 51 is further connected to the output stage of the self-position estimation apparatus 300.
  • the vehicle stop width direction shape memory 52 is connected to a vehicle stop width direction shape calculator 51 and a vehicle stop guidance determination unit 61 described later.
  • the pitch angle corrector 50 removes the influence of the pitch vibration from the second distance information R2i measured by the second distance measuring device 202 based on the inclination angle ⁇ of the road surface with respect to the vehicle body horizontal plane H estimated by the road surface computing unit 40. Correction processing is executed.
  • the vehicle stop width direction shape calculator 51 is a general SLAM (Simultaneous Localization and The shape of the vehicle stop 1002 in the vehicle width direction is calculated by a mapping method such as Mapping.
  • the vehicle stop width direction shape storage unit 52 stores width direction shape information indicating the vehicle width direction shape of the vehicle stop 1002 calculated by the vehicle stop width direction shape calculator 51.
  • the vehicle stop width direction shape calculator 51 may refer to the vehicle stop shape information accumulated in the vehicle stop width direction shape storage unit 52 in order to calculate the width direction shape information more highly.
  • the vehicle stop guidance device 60 includes a vehicle stop guidance determination device 61 and a vehicle body shape storage device 62.
  • the input stage of the car stop guidance determination unit 61 is connected to the output stage of the car stop position calculator 41 and the self-position estimation device 300, and also to the car stop width direction shape memory 52 and the vehicle body shape memory 62, respectively. Connected.
  • the vehicle stop guidance determination unit 61 reads out the information stored in the vehicle stop width direction shape storage unit 52 and the vehicle body shape storage unit 62 as necessary.
  • the vehicle stop guidance determination unit 61 includes the distance to the vehicle stop and the inclination of the vehicle stop estimated by the vehicle stop position calculator 41, the width direction shape of the vehicle stop stored in the vehicle stop width direction shape memory 52, and the turning angular velocity measured by the yaw rate sensor 302. Based on the vehicle body shape (including the size in the vehicle width direction) stored in the vehicle body shape memory 62, the distance for colliding from the dump truck 1 to the vehicle stop and the width direction of the vehicle stop 1002 necessary for guidance determination The direction of the body of the dump truck 1 is calculated.
  • the vehicle stop guidance device 60 is merely an example of an output destination of the vehicle stop detection device 10, and controls autonomous traveling when other devices, for example, the dump truck 1 is a so-called unmanned dump truck that autonomously travels.
  • the autonomous traveling control device may be configured to execute a calculation necessary for stopping.
  • the dump truck 1 may be provided with a collision determination device, and this collision determination device may be used as an output destination of the vehicle stop detection device 10. Then, the collision determination device may perform a collision determination with the vehicle stop and control for avoiding a collision (interference) with the vehicle stop.
  • the vehicle stop detection device 10, the vehicle stop guidance device 60, the reverse assist device 100, and the self-position estimation device 300 are mounted on a controller using a microcomputer device or a general-purpose computer including a central processing unit, a storage device, an input / output circuit, and a communication circuit. It may be configured by a combination of hardware and software that realizes the function of each device, or may be a controller configured by an arithmetic circuit that realizes the function of each device.
  • the self-position estimation apparatus 300 includes a controller (computer) that calculates a vehicle position using an output from a GPS (global positioning system) or an IMU (Internal Measurement Unit).
  • FIG. 5A is a schematic diagram for explaining a vehicle stop detection process by the backward assistance device.
  • FIG. 5B is a schematic diagram in which the measurement results of the first distance measuring device 201 are plotted.
  • the dotted line in FIG. 5A represents the distance to the detected object (including both the road surface and the car stop) measured by the car stop position estimating unit 101.
  • 5A indicates the measurement points measured by the first distance measurement device 201
  • the black dots in FIG. 5B indicate positions where the first distance measurement device 201 outputs the first distance information R1i on the orthogonal coordinates. .
  • the laser beam is irradiated from ⁇ 45 ° to ⁇ 225 ° while the scanner 200 rotates, and from the rear end of the rear wheel of the dump truck 1 to the inclined surface 1002a of the car stop 1002
  • the distance to each point arranged along is calculated and output as first distance information R1i.
  • the line of intersection between the first scan surface 21 and the traveling road surface 1001 becomes shorter.
  • the second scan plane 22 calculates the distance to each point aligned in the width direction on the inclined plane 1002a instead of the traveling road plane 1001 and calculates the second. Output as distance information R2i. Thereafter, as the dump truck 1 further approaches the vehicle stop 1002, the second scan surface 22 moves toward the upper end of the inclined surface 1002a.
  • FIG. 6 is a flowchart showing the processing of the vehicle stop position estimation unit.
  • the road surface computing unit 40 has a first distance information R1i measured by the first distance measuring device 201 and the distance from the road surface road_old estimated by the road surface computing unit 40 in the previous measurement is equal to or less than the first threshold value K1. A certain thing is extracted (S101).
  • step S108 When the number n of the extracted distance information is less than the road threshold Kn (S102 / NO), the road line t road set to an initial value ini_t road (S108), the process proceeds to step S104 to be described later.
  • the road surface calculator 40 calculates a road surface road that approximates the extracted distance information by linear regression (S103). ).
  • linear regression processing for example, linear approximation may be performed by a first-order least square method, or two or more linear approximations or curve approximations may be used. The same applies to linear regression in the calculation of the vehicle stop line described later.
  • the road surface calculator 40 outputs the calculated road surface road to the vehicle stop position calculator 41.
  • the vehicle stop position calculator 41 includes a first stop information R1i measured by the first distance measurement device 201.
  • the vehicle stop position calculator 41 estimates that the distance from the road surface road is equal to or greater than the second threshold K2 and the vehicle stop position calculator 41 estimates.
  • First distance information R1i whose distance from the line t berm_old is equal to or smaller than the third threshold value K3 is extracted (S104).
  • the vehicle stop position calculator 41 ends the process.
  • the vehicle stop position calculator 41 is a vehicle stop line t berm that approximates the extracted first distance information R1i by linear regression. Is calculated (S106).
  • the bollard position calculator 41 calculates the wheel stopper inclination angle Ab from the slope of t road and t berm (S107).
  • FIG. 7 is a flowchart showing a vehicle stop width direction shape detection process by the vehicle stop width direction shape estimation unit.
  • FIG. 8 is a schematic diagram for explaining mapping in the vehicle stop width direction shape detection process.
  • the pitch angle corrector 50 applies the installation depression angle ⁇ (see FIG. 2) of the second distance measuring device 202 and the inclination angle ⁇ of the road surface and the vehicle body horizontal plane H estimated by the road surface calculator 40 to the following equation (1). Then, three-dimensional conversion is performed on the second distance information R2i while correcting the pitch vibration (S201).
  • the vehicle stop width direction shape calculator 51 performs mapping of the second distance information R2i corrected by the pitch angle corrector 50 based on the self-position estimated by the self-position estimation device 300 (S202). Then, the vehicle stop width direction shape calculator 51 writes the width direction shape information composed of the second distance information R2i mapped in the vehicle stop width direction shape memory 52, thereby storing the mapped distance information (S203).
  • a point group R2_c indicates current distance information
  • a point group R2_pre1 and a point group R2_pre2 indicate past width direction shape information. It should be noted that the point group R2_pre2 is the width direction shape information before the point group R2_pre1.
  • the first scan surface 21 is perpendicular to the vehicle body horizontal surface of the dump truck 1 and parallel to the vehicle body longitudinal direction
  • the second scan surface 22 has a depression angle with respect to the vehicle body horizontal surface
  • the second scan surface It is installed in the dump truck 1 so that the intersection line of 22 and the vehicle body horizontal surface of the dump truck 1 is parallel to the rear axle of the dump truck 1.
  • the width shape information input to the vehicle stop width direction shape calculator 51 is obtained when the intersection line between the second scan surface 22 and the vehicle body horizontal plane is parallel to the rear wheel axle (width direction). This is desirable because it can be obtained evenly.
  • FIG. 9A is a schematic diagram for explaining a vehicle stop detection process when pitch vibration is generated in the dump truck 1.
  • 9B and 9C are diagrams showing measurement results performed by the first distance measuring device in the state of FIG. 9A.
  • FIG. 9B is a diagram showing a case where the road surface is flat.
  • the raw data from the first distance measuring device 201 is polar coordinates of the distance L and the laser irradiation angle ⁇ , and these are coordinate-converted into orthogonal coordinates (x, y).
  • FIG. 9C shows a state where the road surface is rotated by an inclination angle ⁇ around the ground directly below the first distance measuring device 201 in the vertical direction.
  • the place measured by the second distance measuring device 202 is a place rotated by ⁇ around the second distance measuring device 202 with reference to a case where there is no pitch vibration.
  • the laser irradiation angle ⁇ is not changed, but the distance L is short (or long).
  • the polar coordinates of the distance L of the raw data from the first distance measuring device 201 and the laser irradiation angle ⁇ are converted into orthogonal coordinates (x, y), they are converted into xy coordinates plotted on the orthogonal coordinates as shown in FIG. 9C.
  • FIG. 9D is a diagram showing the inclination angle ⁇ in the case of two or more linear approximations.
  • FIG. 9E is a diagram showing the inclination angle ⁇ in the case of approximating two or more curves.
  • the road surface calculator 40 extracts the first distance information R1i obtained by measuring the road surface using the previously estimated road surface road_old_old to estimate the road surface, the influence of the pitch vibration is affected even in the situation where the pitch vibration is occurring. It is possible to relax and estimate the slope of the road surface.
  • the vehicle stop position calculator 41 also extracts the first distance information R1i obtained by measuring the vehicle stop using the previously estimated vehicle stop line t berm_old and estimates the vehicle stop, so that the pitch vibration is generated even in the situation where the pitch vibration is generated. The position and inclination of the vehicle stop can be estimated by mitigating the influence of the vehicle.
  • the pitch angle corrector 50 performs correction using the vehicle body horizontal plane H calculated by the road surface computing unit 40 and the road surface inclination angle ⁇ as expressed by the equation (1), the pitch angle corrector 50 is caused by the pitch vibration of the second distance measuring device 202.
  • the depression angle can be corrected.
  • FIG. 10 is a schematic diagram for explaining the backward detection process when the dump truck 1 is traveling on an inclined traveling road surface 1001A.
  • the distance information measured by the first distance measuring device 201 and the second distance measuring device 202 is measured on the traveling road surface 1001 having no inclination shown in FIG. 5A. Will be the same. Accordingly, the distance to the car stop 1002, the car stop angle, and the car stop shape can be accurately estimated even when the vehicle is traveling on the inclined road surface 1001A.
  • the inclination sensor determines the inclination by gravity. Therefore, when traveling on the traveling road surface 1001A having the inclination as shown in FIG. As a result, it is determined that there is noise, and accurate pitch vibration correction cannot be performed.
  • the pitch angle corrector 50 uses the relative inclination angle between the vehicle body horizontal surface H of the dump truck 1 and the traveling road surface 1001A, the pitch vibration can be corrected regardless of the inclination of the road surface. Therefore, when the vehicle is traveling on a traveling road surface where the inclination changes, the pitch angle corrector 50 can correct not only the pitch vibration but also the change in the road surface inclination.
  • the scanning surfaces of the first distance measuring device 201 and the second distance measuring device 202 are crossed and installed on the dump truck 1 so that when the pitch vibration is generated in the dump truck 1 or on the road surface Even when there is an inclination, it is possible to reduce the influence of pitch vibration and measure the distance to the car stop, the shape and direction of the car stop (the inclination angle of the car stop and the shape in the width direction). Thereby, it can stop at an appropriate earthing position at the time of earthing. For example, it is possible to obtain determination information for guiding the direction of the vehicle body of the dump truck so that the vehicle width direction of the dump truck follows the width direction shape of the vehicle stop.
  • the work machine is not limited to a dump truck, and may be a dozer, a wheel loader, or a hydraulic excavator.
  • a scanner may be attached to the rear of these work machines to detect the shape of a rear obstacle.
  • a scanner may be installed in front, left, and right of the work machine to detect the shape of the front obstacle, the left obstacle, and the right obstacle.

Abstract

This scanner for detecting an obstacle includes a first distance measuring device and a second distance measuring device. The first distance measuring device and the second distance measuring device each consist of a laser ranging scanner which measures the distance to an object being detected by radiating a laser and receiving reflected light that has been reflected by the object being detected. The first distance measuring device and the second distance measuring device are disposed in such a way that a laser radiating surface of the first distance measuring device and a laser radiating surface of the second distance measuring device intersect one another at right angles.

Description

スキャナー、作業機械、及び車止め検出装置Scanner, work machine, and vehicle stop detection device
　本発明は、スキャナー、作業機械、及び作業機械の車止め検出装置に関する。 The present invention relates to a scanner, a work machine, and a vehicle stop detection device for the work machine.
　特許文献１には、「道路等の表面の凹凸検査は水準プロフィール測定を簡便かつ迅速に行う方法を提供する」ことを目的として「交差する少なくとも２つの光平面を用いる光操作方法（要約抜粋）」が開示されている。 Patent Document 1 states that “an optical operation method using at least two intersecting light planes” (summary excerpt) for the purpose of “providing a simple and quick method of level profile measurement for surface roughness inspection of roads and the like” Is disclosed.
　また特許文献２には、複数の障害物センサをダンプトラックに搭載し、各障害物センサの路面上のスキャン範囲を異ならせて障害物を検出する技術が開示されている。 Also, Patent Document 2 discloses a technique for detecting an obstacle by mounting a plurality of obstacle sensors on a dump truck and varying the scanning range on the road surface of each obstacle sensor.
特開２００１－２２１６２０号公報JP 2001-221620 A 米国特許公開第００９２５９３９９号明細書US Patent Publication No. 009259399
　鉱山用ダンプトラックが積荷を放土する際に、放土場に設置された車止めに向けて後退で接近し、車止め直前で停止をする動作を行うことがある。この車止めはダンプトラックのタイヤ止めとしての機能を果たせるための地上高を有して形成されるが、車止めが盛り土で形成されるために個々で形状が異なっていたり、また仮に車止めの形状を一定に形成していても盛り土が崩れることで形状が変化したりすることがある。よって、車止めの直前でダンプトラックを停止させるためには車止めの位置や形状を的確に検出したいという要望がある。 When the dump truck for mining releases the load, it may move backward toward the car stop installed at the earth release site and stop immediately before the car stops. This car stop is formed with a ground clearance to function as a dump truck tire stop, but because the car stop is formed with embankment, the shape is different individually, and the shape of the car stop is constant Even if it is formed, the shape may change due to the collapse of the embankment. Therefore, in order to stop the dump truck immediately before the car stop, there is a demand for accurately detecting the position and shape of the car stop.
　特許文献１、特許文献２は共に複数の障害物センサを用いて路面をスキャンしているが、両文献とも路面に対して高さを有する車止めの高さや形状を検出することは考慮されていない。そのため、例えば特許文献１では「照射光は、交点２Ｌ、ＡＬ、ＢＬが造る光平面１０Ｌ、および交点２Ｒ、ＡＲ、ＢＲが造る光平面１０Ｒを走査するように照射され、路面６では線７Ｌ及び線７Ｒに沿って反射して反射光となる」（特許文献１の段落０００８参照）場合には線７Ｌ及び線７Ｒの間が死角となり、ここに車止めがある場合には車止めまでの距離及び形状が検出できないという不具合が生じる。また特許文献２では、ダンプトラックの幅方向に沿ってスキャン面４０ａ、４０ｂをずらしている。これをダンプトラックの後方監視に適用した場合、スキャン面４０ａ、４０ｂ共にダンプトラック近傍から遠方にかけての各スキャン面と路面との交線上の距離情報しか取得できず、車止めの形状、特にダンプトラックの車幅方向に沿った車止めの形状は取得できない。よって、万一車止めの一部が壊れて盛り土がダンプトラック手前に堆積していても、堆積土にスキャン面が重ならなければ堆積土の高さや壊れた車止めの形状を検出できず、ダンプトラックが堆積土に乗り上げてしまうといった不具合が生じる。よって、単に車止めの位置を検出するだけではなく、車止め形状の検出に適した技術の開発が望まれているという実情がある。 Both Patent Literature 1 and Patent Literature 2 scan the road surface using a plurality of obstacle sensors, but neither literature considers detecting the height or shape of a car stop having a height relative to the road surface. . Therefore, for example, in Patent Document 1, “irradiation light is irradiated so as to scan an optical plane 10L formed by the intersections 2L, AL, and BL and an optical plane 10R formed by the intersections 2R, AR, and BR. In the case of “reflected along the line 7R to be reflected light” (see paragraph 0008 of Patent Document 1), a blind spot is formed between the line 7L and the line 7R. If there is a car stop here, the distance and shape to the car stop This causes a problem that cannot be detected. In Patent Document 2, the scan surfaces 40a and 40b are shifted along the width direction of the dump truck. When this is applied to the rear monitoring of the dump truck, both the scanning surfaces 40a and 40b can only acquire distance information on the intersection line between the scanning surface and the road surface from the vicinity of the dump truck to the distance. The shape of the car stop along the vehicle width direction cannot be acquired. Therefore, even if a part of the car stop breaks and the embankment accumulates in front of the dump truck, the height of the deposited soil and the shape of the broken car stop cannot be detected unless the scan surface overlaps the piled earth. This causes a problem such that the mine rides on sedimentary soil. Therefore, there is a situation where it is desired to develop a technique suitable not only for detecting the position of the car stop but also for detecting the car stop shape.
　本発明は上記実情に鑑みてなされたものであり、特に鉱山において設置された車止め形状の検出に適した技術を提供することを目的とする。 The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a technique suitable for detecting a car stop shape installed particularly in a mine.
　上記課題を達成するために、本発明は特許請求の範囲に記載の構成を備える。その一例をあげるならば、本発明は障害物を検出するスキャナーであって、前記スキャナーは、第一距離計測装置及び第二距離計測装置を含み、前記第一距離計測装置及び前記第二距離計測装置の其々は、レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定するレーザレンジスキャナにより構成され、前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面及び前記第二距離計測装置のレーザ照射面を直交させて配置される、ことを特徴とする。 In order to achieve the above object, the present invention has the structure described in the claims. For example, the present invention is a scanner for detecting an obstacle, and the scanner includes a first distance measuring device and a second distance measuring device, and the first distance measuring device and the second distance measuring device. Each of the devices includes a laser range scanner that irradiates a laser, receives reflected light reflected by the detected object, and measures a distance to the detected object, and includes the first distance measuring device and the first distance measuring device. The two-distance measuring device is characterized in that the laser irradiation surface of the first distance measuring device and the laser irradiation surface of the second distance measuring device are arranged orthogonal to each other.
　本発明によれば特に鉱山において設置された車止め形状の検出に適した技術を提供することができる。上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。 According to the present invention, it is possible to provide a technique suitable for detecting a car stop shape installed particularly in a mine. Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.
本実施形態に係るダンプトラックの放土時の周辺環境を示す説明図Explanatory drawing which shows the surrounding environment at the time of earthmoving of the dump truck concerning this embodiment ダンプトラックの左側面図Dump truck left side view ダンプトラックの後方斜視図Rear perspective view of dump truck ダンプトラックの機能構成を示すブロック図Block diagram showing the functional configuration of the dump truck 車止め検出処理を説明するための模式図（ダンプトラック左側面視）Schematic diagram for explaining the car stop detection process (dump truck left side view) 第一距離計測装置の計測結果をプロットした模式図Schematic diagram plotting the measurement results of the first distance measuring device 車止め位置推定部の処理を示すフローチャートFlowchart showing processing of the car stop position estimation unit 車止め幅方向形状推定部による車止め幅方向形状検出処理を示すフローチャートThe flowchart which shows the vehicle stop width direction shape detection process by the vehicle stop width direction shape estimation part 車止め幅方向形状検出処理におけるマッピングを説明するための図（ダンプトラックの上面視）Diagram for explaining mapping in car stop width direction shape detection processing (top view of dump truck) ダンプトラックにピッチ振動が発生している場合の車止め検出処理を説明するための模式図（ダンプトラックの側面視）Schematic diagram for explaining the vehicle stop detection process when pitch vibration is generated in the dump truck (side view of the dump truck) 路面が平面な場合を示す図Diagram showing the case where the road surface is flat 路面が第一距離計測装置の鉛直方向真下の地面を中心として、傾斜角θ分回転した状態を示す図The figure which shows the state which the road surface rotated by inclination-angle (theta) centering on the ground right under the perpendicular direction of a 1st distance measuring device ２本以上の直線近似の場合における傾斜角θを示す図The figure which shows inclination-angle (theta) in the case of two or more linear approximations は、２本以上曲線近似の場合における傾斜角θを示す図Is a diagram showing the inclination angle θ in the case of approximation of two or more curves ダンプトラックが傾斜を持つ走行路面上を走行している場合の後方検出処理を説明するための模式図（ダンプトラックの側面視）Schematic diagram for explaining the rear detection process when the dump truck is traveling on an inclined road surface (side view of the dump truck)
　以下、本発明の実施形態を図面に基づいて詳細に説明する。なお、実施形態を説明するための全図において、同一の機能を有する部材には同一または関連する符号を付し、その繰り返しの説明は省略する。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In all the drawings for explaining the embodiments, members having the same function are denoted by the same or related reference numerals, and repeated description thereof is omitted.
　図１から図３を参照してダンプトラックの概略構成について説明する。図１は、本実施形態に係るダンプトラック１の放土時の周辺環境を示す説明図である。図２は、ダンプトラック１の左側面図である。図３は、ダンプトラック１の後方斜視図である。 The schematic configuration of the dump truck will be described with reference to FIGS. FIG. 1 is an explanatory diagram showing the surrounding environment when the dump truck 1 according to the present embodiment is released. FIG. 2 is a left side view of the dump truck 1. FIG. 3 is a rear perspective view of the dump truck 1.
　図１に示すように、ダンプトラック１は崖下にある放土場１００３に放土する際、崖際に配置された車止め１００２に向かって走行路面１００１上を後退走行する。ダンプトラック１は、車体フレーム２（図２参照）の前下部に右前輪３ｒ及び左前輪３ｌ（図２参照）を備え、後下部に右後輪４ｒ及び左後輪４ｌを備える。更に車体フレーム２の後部にはリアアスクル７が備えられる。一方、車体フレーム２の前上部には運転席５（図２参照）が備えられ、後ろ上部にはベッセル６が備えられる。 As shown in FIG. 1, the dump truck 1 travels backward on the traveling road surface 1001 toward the car stop 1002 disposed on the cliff side when the dump truck 1 is earthed at the earth ground 1003 under the cliff. The dump truck 1 includes a right front wheel 3r and a left front wheel 3l (see FIG. 2) at a front lower part of a body frame 2 (see FIG. 2), and a right rear wheel 4r and a left rear wheel 4l at a rear lower part. Further, a rear axle 7 is provided at the rear portion of the vehicle body frame 2. On the other hand, a driver's seat 5 (see FIG. 2) is provided at the front upper part of the body frame 2, and a vessel 6 is provided at the rear upper part.
　更にダンプトラック１は、複数のレーザスキャナを含むスキャナー２００と、ダンプトラック１の自己位置を検出する自己位置推定装置３００と、スキャナー２００からのセンサ出力及び自己位置推定装置３００からの自己位置を基にダンプトラック１の後退走行を支援する後退支援装置１００とを備える。以下の説明において、後退支援装置１００が動作を始めた時のダンプトラック１の位置を、直交座標系４００の原点とし、後退支援装置１００が動作を始めた瞬間の進行方向をｙ軸とする。また、ダンプトラック１の車体高さ軸をｚ軸とし、ｙ軸及びｚ軸の２軸に直交する軸をｘ軸とする。 Further, the dump truck 1 is based on a scanner 200 including a plurality of laser scanners, a self-position estimation device 300 that detects the self-position of the dump truck 1, a sensor output from the scanner 200, and a self-position from the self-position estimation device 300. And a reverse support device 100 that supports the reverse travel of the dump truck 1. In the following description, the position of the dump truck 1 when the backward support device 100 starts operating is the origin of the orthogonal coordinate system 400, and the traveling direction at the moment when the backward support device 100 starts operating is the y axis. Further, the vehicle body height axis of the dump truck 1 is defined as the z axis, and the axis orthogonal to the two axes of the y axis and the z axis is defined as the x axis.
　スキャナー２００は、第一距離計測装置２０１及び第二距離計測装置２０２を含んで構成される。スキャナー２００は、ダンプトラック１の車体後方、より詳しくはリアアスクル７より上かつベッセル６より下で、左後輪４ｌ及び右後輪４ｒの間に設置される。第一距離計測装置２０１及び第二距離計測装置２０２は、同じ高さに配置される必要はなく、また左右対称でなくてもよい。 The scanner 200 includes a first distance measuring device 201 and a second distance measuring device 202. The scanner 200 is installed between the left rear wheel 4l and the right rear wheel 4r at the rear of the body of the dump truck 1, more specifically, above the rear axle 7 and below the vessel 6. The first distance measuring device 201 and the second distance measuring device 202 do not need to be arranged at the same height and may not be symmetrical.
　第一距離計測装置２０１及び第二距離計測装置２０２は、例えばレーザ光を扇状に照射し、被検出体からの反射光を受光して被検出体までの距離と方向を検出するレーザスキャナ（レーザレンジスキャナ）である。スキャナー２００は、第二距離計測装置２０２のレーザ照射面（以下「第二スキャン面」という）２２を第一距離計測装置２０１のレーザ照射面（以下「第一スキャン面」という）２１と直交させて、第二距離計測装置２０２及び第一距離計測装置２０１を配置して構成される。なおここでいう直交は、厳密に第一スキャン面２１及び第二スキャン面２２の法線ベクトルが垂直となる場合に限定されず、第一距離計測装置２０１が検出した被検出体と同じ被検出体の幅方向の形状が検出できる向きに第二スキャン面２２が向くように、第一距離計測装置２０１及び第二距離計測装置２０２が配置されている場合も含む。 The first distance measuring device 201 and the second distance measuring device 202 are, for example, a laser scanner (laser) that irradiates a laser beam in a fan shape, receives reflected light from the detected object, and detects the distance and direction to the detected object. Range scanner). The scanner 200 makes the laser irradiation surface (hereinafter referred to as “second scan surface”) 22 of the second distance measurement device 202 orthogonal to the laser irradiation surface (hereinafter referred to as “first scan surface”) 21 of the first distance measurement device 201. The second distance measuring device 202 and the first distance measuring device 201 are arranged. Note that the orthogonality is not limited to the case where the normal vectors of the first scan plane 21 and the second scan plane 22 are strictly perpendicular, but the same detected object as the detected object detected by the first distance measuring device 201. This includes the case where the first distance measuring device 201 and the second distance measuring device 202 are arranged so that the second scan surface 22 faces in a direction in which the shape in the width direction of the body can be detected.
　より詳しくは、本実施形態では、第一スキャン面２１が車体フレーム２の前後軸及び左右軸を含む車体水平面Ｈに対して少なくとも垂直下向きから車体後方に向かって車体水平面Ｈと平行な方向までを覆うように第一距離計測装置２０１がダンプトラック１に設置される。 More specifically, in the present embodiment, the first scanning surface 21 extends at least vertically downward from the vehicle body horizontal plane H including the front and rear axes and the left and right axes of the vehicle body frame 2 to the direction parallel to the vehicle body horizontal plane H toward the rear of the vehicle body. The first distance measuring device 201 is installed on the dump truck 1 so as to cover it.
　第二距離計測装置２０２は、走行路面１００１が平坦である時に、第二スキャン面２２と走行路面１００１とが交差する位置Ｐ１と、左後輪４ｌ及び右後輪４ｒの最後端を走行路面１００１に投影した位置Ｐ２との距離Ｄ１が、ダンプトラック１の最大後退速度の制動距離以上となるよう、車体水平面Ｈに対して設置俯角α（図２）を持つようにダンプトラック１に設置される。このように設置することにより、最大後退速度で後退しても車止めに衝突することなく制動が可能である。 When the traveling road surface 1001 is flat, the second distance measuring device 202 uses a position P1 where the second scanning surface 22 and the traveling road surface 1001 intersect, and the rear end of the left rear wheel 4l and the right rear wheel 4r as the traveling road surface 1001. Is installed on the dump truck 1 so as to have an installation angle α (FIG. 2) with respect to the horizontal plane H of the vehicle body so that the distance D1 to the position P2 projected onto the vehicle is equal to or greater than the braking distance of the maximum reverse speed of the dump truck 1. . By installing in this way, braking is possible without colliding with the vehicle stop even if the vehicle moves backward at the maximum reverse speed.
　また第二距離計測装置２０２は、第二距離計測装置２０２の検出距離と第二距離計測装置２０２の車体水平面Ｈに対する設置俯角αと、第二スキャン面２２に対するダンプトラック１の遮蔽によって決まる、走行路面１００１と第二スキャン面２２との交線の長さＤ２（図３参照）がダンプトラック１の車輪幅（外幅）よりも長くなるような位置に設置される。このように設置することにより、ダンプトラックの車輪が通過する領域を前もってスキャンすることが可能である。 The second distance measuring device 202 is determined by the detection distance of the second distance measuring device 202, the installation depression angle α of the second distance measuring device 202 with respect to the vehicle body horizontal plane H, and the shielding of the dump truck 1 with respect to the second scan surface 22. It is installed at a position where the length D2 (see FIG. 3) of the intersection line between the road surface 1001 and the second scan surface 22 is longer than the wheel width (outer width) of the dump truck 1. By installing in this way, it is possible to scan in advance the area through which the wheels of the dump truck pass.
　なお第一距離計測装置２０１及び第二距離計測装置２０２は１つずつ備える構成に限定されず、各計測装置が複数のレーザスキャナを用いて構成されてもよい。また第一距離計測装置２０１に対して第一スキャン面２１は一つに限定されず複数あってもよい。同様に第二距離計測装置２０２に対して第二スキャン面２２は一つに限定されず複数あってもよい。 Note that the first distance measuring device 201 and the second distance measuring device 202 are not limited to the configuration provided one by one, and each measuring device may be configured using a plurality of laser scanners. Further, the first scanning surface 21 is not limited to one for the first distance measuring device 201, and a plurality of first scanning surfaces 21 may be provided. Similarly, the second scanning surface 22 is not limited to one for the second distance measuring device 202 and may be plural.
　図４は、ダンプトラック１の機能構成を示すブロック図である。 FIG. 4 is a block diagram showing a functional configuration of the dump truck 1.
　ダンプトラック１は、車輪速センサ３０１、及びヨーレートセンサ（旋回角速度センサ）３０２を含む。自己位置推定装置３００は車輪速センサ３０１及びヨーレートセンサ３０２の其々に接続され、車輪速センサ３０１から車輪回転速度を取得し、ヨーレートセンサ３０２からダンプトラック１の旋回角速度の計測結果を取得する。そして自己位置推定装置３００は、例えばデットレコニング処理を実行することにより、ダンプトラック１の速度、角速度、及び既述の直交座標系４００（図１参照）の原点からの位置、姿勢を算出する。なお、車体速度を計測する目的で、車輪速センサ３０１に代わりＩＭＵ（慣性計測装置）を、旋回角速度を推定する目的でヨーレートセンサ３０２に代わり操舵角センサを用いてもよい。また、直交座標系４００の原点からの位置、姿勢を計測する目的で、自己位置推定装置３００に代わり、ＧＰＳ（全地球測位システム）と地磁気センサを用いてもよい。 The dump truck 1 includes a wheel speed sensor 301 and a yaw rate sensor (turning angular velocity sensor) 302. The self-position estimation apparatus 300 is connected to each of the wheel speed sensor 301 and the yaw rate sensor 302, acquires the wheel rotation speed from the wheel speed sensor 301, and acquires the measurement result of the turning angular velocity of the dump truck 1 from the yaw rate sensor 302. The self-position estimation apparatus 300 calculates the speed and angular velocity of the dump truck 1 and the position and orientation from the origin of the aforementioned orthogonal coordinate system 400 (see FIG. 1) by executing, for example, dead reckoning processing. Note that an IMU (inertia measurement device) may be used instead of the wheel speed sensor 301 for the purpose of measuring the vehicle body speed, and a steering angle sensor may be used instead of the yaw rate sensor 302 for the purpose of estimating the turning angular speed. Further, for the purpose of measuring the position and orientation from the origin of the orthogonal coordinate system 400, a GPS (Global Positioning System) and a geomagnetic sensor may be used instead of the self-position estimation apparatus 300.
　後退支援装置１００（後退支援コントローラ）は、車止め検出装置１０（車止め検出コントローラ）及び車止め誘導装置６０（車止め誘導コントローラ）を含んで構成される。車止め検出装置１０は、車止め位置推定部１０１及び車止め幅方向形状推定部１０２を含む。また車止め誘導装置６０は、車止め誘導判断器６１及び車体形状記憶器６２を含む。 The reverse assistance device 100 (reverse assistance controller) includes a vehicle stop detection device 10 (vehicle stop detection controller) and a vehicle stop guidance device 60 (vehicle stop guidance controller). The vehicle stop detection device 10 includes a vehicle stop position estimation unit 101 and a vehicle stop width direction shape estimation unit 102. The vehicle stop guidance device 60 includes a vehicle stop guidance determination device 61 and a vehicle body shape storage device 62.
　車止め位置推定部１０１は、路面線演算器４０及び車止め位置演算器４１を含む。既述の第一距離計測装置２０１の出力段は、路面線演算器４０及び車止め位置演算器４１の其々の入力段に接続される。路面線演算器４０の出力段は車止め位置演算器４１及び後述するピッチ角補正器５０に接続される。車止め位置演算器４１の入力段には第一距離計測装置２０１の出力段が更に接続される。 The car stop position estimation unit 101 includes a road surface calculator 40 and a car stop position calculator 41. The output stage of the first distance measuring device 201 described above is connected to the input stages of the road surface line calculator 40 and the vehicle stop position calculator 41. The output stage of the road surface calculator 40 is connected to a car stop position calculator 41 and a pitch angle corrector 50 described later. The output stage of the first distance measuring device 201 is further connected to the input stage of the car stop position calculator 41.
　第一距離計測装置２０１が出力する第一距離情報Ｒ１ｉ（ｉ：第一距離計測装置２０１におけるレーザ照射角度φを示し、例えば－４５≦ｉ≦２２５、後述する第二距離情報Ｒ２ｉも同様）は、測定点及び測定点までの距離が含まれる。路面線演算器４０は、第一距離情報Ｒ１ｉの内、路面と推定される測定点及び距離を示す距離情報を抽出し、路面線を演算する。そして車体フレーム２の前後軸及び左右軸を含む車体水平面Ｈに対する路面線の傾斜角θ（図９Ａ参照）を演算し、車体水平面Ｈに対する路面の傾きを算出する。傾斜角θをピッチ角θともいう。 The first distance information R1i output from the first distance measuring device 201 (i: indicates the laser irradiation angle φ in the first distance measuring device 201, for example, −45 ≦ i ≦ 225, the same applies to the second distance information R2i described later). , The measurement point and the distance to the measurement point are included. The road line calculator 40 extracts distance information indicating the measurement point and distance estimated as the road surface from the first distance information R1i, and calculates the road line. Then, the inclination angle θ (see FIG. 9A) of the road surface line with respect to the vehicle body horizontal plane H including the front and rear axes and the left and right axes of the vehicle body frame 2 is calculated, and the road surface inclination with respect to the vehicle body horizontal plane H is calculated. The inclination angle θ is also referred to as a pitch angle θ.
　車止め位置演算器４１は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉの内、車止めと推定されるものの距離情報を抽出し、路面及び車止めの変曲点と、路面に対する車止めの傾斜を算出する。 The vehicle stop position calculator 41 extracts the distance information of what is estimated to be a vehicle stop from the first distance information R1i measured by the first distance measuring device 201, the inflection point of the road surface and the vehicle stop, and the inclination of the vehicle stop with respect to the road surface. Is calculated.
　車止め幅方向形状推定部１０２は、ピッチ角補正器５０、車止め幅方向形状演算器５１、及び車止め幅方向形状記憶器５２を含む。ピッチ角補正器５０の入力段は、第二距離計測装置２０２及び路面線演算器４０の其々の出力段に接続される。ピッチ角補正器５０の出力段は車止め幅方向形状演算器５１の入力段に接続される。車止め幅方向形状演算器５１の入力段は、更に自己位置推定装置３００の出力段に接続される。車止め幅方向形状記憶器５２は、車止め幅方向形状演算器５１及び後述する車止め誘導判断器６１に接続される。 The car stop width direction shape estimation unit 102 includes a pitch angle corrector 50, a car stop width direction shape calculator 51, and a car stop width direction shape memory 52. The input stage of the pitch angle corrector 50 is connected to the respective output stages of the second distance measuring device 202 and the road surface calculator 40. The output stage of the pitch angle corrector 50 is connected to the input stage of the vehicle stop width direction shape calculator 51. The input stage of the vehicle stop width direction shape calculator 51 is further connected to the output stage of the self-position estimation apparatus 300. The vehicle stop width direction shape memory 52 is connected to a vehicle stop width direction shape calculator 51 and a vehicle stop guidance determination unit 61 described later.
　ピッチ角補正器５０は、路面線演算器４０が推定した車体水平面Ｈに対する路面の傾斜角θに基づいて、第二距離計測装置２０２が計測した第二距離情報Ｒ２ｉから、ピッチ振動による影響を取り除くための補正処理を実行する。 The pitch angle corrector 50 removes the influence of the pitch vibration from the second distance information R2i measured by the second distance measuring device 202 based on the inclination angle θ of the road surface with respect to the vehicle body horizontal plane H estimated by the road surface computing unit 40. Correction processing is executed.
　車止め幅方向形状演算器５１は、ピッチ角補正器５０が補正した第二距離情報Ｒ２ｉと自己位置推定装置３００が推定したダンプトラック１の位置及び姿勢に基づいて、一般的なＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）等のマッピング方法によって、車止め１００２の車幅方向の形状を演算する。 Based on the second distance information R2i corrected by the pitch angle corrector 50 and the position and posture of the dump truck 1 estimated by the self-position estimation device 300, the vehicle stop width direction shape calculator 51 is a general SLAM (Simultaneous Localization and The shape of the vehicle stop 1002 in the vehicle width direction is calculated by a mapping method such as Mapping.
　車止め幅方向形状記憶器５２は、車止め幅方向形状演算器５１が演算した車止め１００２の車幅方向の形状を示す幅方向形状情報を蓄積する。車止め幅方向形状演算器５１はより高度に幅方向形状情報を算出するために、車止め幅方向形状記憶器５２が蓄積した車止め形状情報を参照してもよい。 The vehicle stop width direction shape storage unit 52 stores width direction shape information indicating the vehicle width direction shape of the vehicle stop 1002 calculated by the vehicle stop width direction shape calculator 51. The vehicle stop width direction shape calculator 51 may refer to the vehicle stop shape information accumulated in the vehicle stop width direction shape storage unit 52 in order to calculate the width direction shape information more highly.
　車止め誘導装置６０は、車止め誘導判断器６１及び車体形状記憶器６２を含む。車止め誘導判断器６１の入力段は、車止め位置演算器４１、自己位置推定装置３００のそれぞれの出力段に接続されると共に、車止め幅方向形状記憶器５２、車体形状記憶器６２の其々にも接続される。車止め誘導判断器６１は、車止め幅方向形状記憶器５２及び車体形状記憶器６２に記憶された情報を必要に応じて読み出す。 The vehicle stop guidance device 60 includes a vehicle stop guidance determination device 61 and a vehicle body shape storage device 62. The input stage of the car stop guidance determination unit 61 is connected to the output stage of the car stop position calculator 41 and the self-position estimation device 300, and also to the car stop width direction shape memory 52 and the vehicle body shape memory 62, respectively. Connected. The vehicle stop guidance determination unit 61 reads out the information stored in the vehicle stop width direction shape storage unit 52 and the vehicle body shape storage unit 62 as necessary.
　車止め誘導判断器６１は、車止め位置演算器４１が推定した車止めまでの距離及び車止めの傾斜と、車止め幅方向形状記憶器５２が記憶する車止めの幅方向形状と、ヨーレートセンサ３０２が計測する旋回角速度と、車体形状記憶器６２が記憶する車体形状（車幅方向の大きさを含む）に基づいて、誘導の判断に必要なダンプトラック１から車止めに衝突するための距離及び車止め１００２の幅方向に対するダンプトラック１の車体の向きを算出する。 The vehicle stop guidance determination unit 61 includes the distance to the vehicle stop and the inclination of the vehicle stop estimated by the vehicle stop position calculator 41, the width direction shape of the vehicle stop stored in the vehicle stop width direction shape memory 52, and the turning angular velocity measured by the yaw rate sensor 302. Based on the vehicle body shape (including the size in the vehicle width direction) stored in the vehicle body shape memory 62, the distance for colliding from the dump truck 1 to the vehicle stop and the width direction of the vehicle stop 1002 necessary for guidance determination The direction of the body of the dump truck 1 is calculated.
　本実施形態において車止め誘導装置６０は、車止め検出装置１０の出力先の一例に過ぎず、他の装置、例えばダンプトラック１が自律走行をする所謂無人ダンプトラックである場合には、自律走行を制御する自律走行制御装置に出力して、ダンプトラック１から車止めまでの距離や、車止めに対するダンプトラック１の車体の向き（車体水平面の向き、車体前後軸、また車体左右軸の向きでもよい）等、停車に必要な演算を自律走行制御装置が実行するように構成されてもよい。更に、ダンプトラック１に衝突判定装置を備え、この衝突判定装置を車止め検出装置１０の出力先として用いてもよい。そして衝突判定装置が車止めとの衝突判定を行い、車止めとの衝突（干渉）を回避するための制御を行ってもよい。 In the present embodiment, the vehicle stop guidance device 60 is merely an example of an output destination of the vehicle stop detection device 10, and controls autonomous traveling when other devices, for example, the dump truck 1 is a so-called unmanned dump truck that autonomously travels. The distance from the dump truck 1 to the car stop, the direction of the body of the dump truck 1 relative to the car stop (the direction of the horizontal plane of the car body, the front and rear axes of the car body, or the left and right axis of the car body), etc. The autonomous traveling control device may be configured to execute a calculation necessary for stopping. Further, the dump truck 1 may be provided with a collision determination device, and this collision determination device may be used as an output destination of the vehicle stop detection device 10. Then, the collision determination device may perform a collision determination with the vehicle stop and control for avoiding a collision (interference) with the vehicle stop.
　車止め検出装置１０、車止め誘導装置６０、後退支援装置１００及び自己位置推定装置３００は、中央演算装置、記憶装置、入出力回路、及び通信回路を含むマイコン装置や汎用のコンピュータを用いたコントローラに実装され、ハードウェアと各装置の機能を実現するソフトウェアとの組み合わせにより構成してもよいし、各装置の機能を実現する演算回路により構成されたコントローラでもよい。また自己位置推定装置３００はＧＰＳ（ｇｌｏｂａｌ　ｐｏｓｉｔｉｏｎｉｎｇ　ｓｙｓｔｅｍ）やＩＭＵ（Ｉｎｅｒｔｉａｌ　Ｍｅａｓｕｒｅｍｅｎｔ　Ｕｎｉｔ）の出力を用いて車両位置を演算するコントローラ（コンピュータ）により構成される。 The vehicle stop detection device 10, the vehicle stop guidance device 60, the reverse assist device 100, and the self-position estimation device 300 are mounted on a controller using a microcomputer device or a general-purpose computer including a central processing unit, a storage device, an input / output circuit, and a communication circuit. It may be configured by a combination of hardware and software that realizes the function of each device, or may be a controller configured by an arithmetic circuit that realizes the function of each device. The self-position estimation apparatus 300 includes a controller (computer) that calculates a vehicle position using an output from a GPS (global positioning system) or an IMU (Internal Measurement Unit).
　次に図５Ａ及び図５Ｂを参照してスキャナー２００を用いてダンプトラック１の後方にある車止めを検出する処理について説明する。図５Ａは後退支援装置による車止め検出処理を説明するための模式図である。図５Ｂは第一距離計測装置２０１の計測結果をプロットした模式図である。 Next, a process of detecting a vehicle stop behind the dump truck 1 using the scanner 200 will be described with reference to FIGS. 5A and 5B. FIG. 5A is a schematic diagram for explaining a vehicle stop detection process by the backward assistance device. FIG. 5B is a schematic diagram in which the measurement results of the first distance measuring device 201 are plotted.
　図５Ａの点線は、車止め位置推定部１０１が計測した被検出体（路面、車止めの両方を含む）までの距離を表す。また図５Ａの黒点は、第一距離計測装置２０１が計測した測定点を示し、図５Ｂの黒点は、直交座標上での第一距離計測装置２０１が第一距離情報Ｒ１ｉを出力した位置を示す。ダンプトラック１が車止め１００２にむかって後退を始めると、スキャナー２００が回転しながら－４５°～－２２５°にレーザを照射してダンプトラック１の後輪最後端から車止め１００２の傾斜面１００２ａまでに沿って並ぶ各点までの距離を算出し、第一距離情報Ｒ１ｉとして出力する。ダンプトラック１が車止め１００２に接近するにつれて、第一スキャン面２１と走行路面１００１上との交線は短くなる。 The dotted line in FIG. 5A represents the distance to the detected object (including both the road surface and the car stop) measured by the car stop position estimating unit 101. 5A indicates the measurement points measured by the first distance measurement device 201, and the black dots in FIG. 5B indicate positions where the first distance measurement device 201 outputs the first distance information R1i on the orthogonal coordinates. . When the dump truck 1 starts to move backward toward the car stop 1002, the laser beam is irradiated from −45 ° to −225 ° while the scanner 200 rotates, and from the rear end of the rear wheel of the dump truck 1 to the inclined surface 1002a of the car stop 1002 The distance to each point arranged along is calculated and output as first distance information R1i. As the dump truck 1 approaches the vehicle stop 1002, the line of intersection between the first scan surface 21 and the traveling road surface 1001 becomes shorter.
　図５Ｂに示すように第一距離情報Ｒ１ｉを横軸が水平方向位置、縦軸が垂直方向高さを示す座標系にプロットすると、水平方向にほぼ平行な直線と、垂直方向高さが変化する直線とが得られる。この二直線の交点が第一距離情報Ｒ１ｉの変曲点であり、路面と車止めとが接する点と推定できる。 As shown in FIG. 5B, when the first distance information R1i is plotted on a coordinate system in which the horizontal axis indicates the horizontal position and the vertical axis indicates the vertical height, a straight line substantially parallel to the horizontal direction and the vertical height change. A straight line is obtained. The intersection of these two straight lines is the inflection point of the first distance information R1i, and it can be estimated that the road surface and the vehicle stop are in contact with each other.
　また図５Ａではダンプトラック１が車止め１００２に十分接近しているので、第二スキャン面２２は、走行路面１００１ではなく傾斜面１００２ａ上の幅方向に並ぶ各点までの距離を算出して第二距離情報Ｒ２ｉとして出力する。以後、ダンプトラック１が車止め１００２に更に接近するにつれて、第二スキャン面２２は傾斜面１００２ａの上端に向かって移動する。 In FIG. 5A, since the dump truck 1 is sufficiently close to the car stop 1002, the second scan plane 22 calculates the distance to each point aligned in the width direction on the inclined plane 1002a instead of the traveling road plane 1001 and calculates the second. Output as distance information R2i. Thereafter, as the dump truck 1 further approaches the vehicle stop 1002, the second scan surface 22 moves toward the upper end of the inclined surface 1002a.
　次に図６を参照して車止め位置推定部１０１の動作について説明する。図６は車止め位置推定部の処理を示すフローチャートである。 Next, the operation of the vehicle stop position estimation unit 101 will be described with reference to FIG. FIG. 6 is a flowchart showing the processing of the vehicle stop position estimation unit.
　路面線演算器４０は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉのうち、前回の計測で路面線演算器４０が推定した路面線ｔｒｏａｄ＿ｏｌｄとの距離が第一閾値Ｋ１以下であるものを抽出する（Ｓ１０１）。 The road surface computing unit 40 has a first distance information R1i measured by the first distance measuring device 201 and the distance from the road surface road_old estimated by the road surface computing unit 40 in the previous measurement is equal to or less than the first threshold value K1. A certain thing is extracted (S101).
　抽出した距離情報の個数ｎが路面閾値Ｋｎ未満である場合（Ｓ１０２／ＮＯ）、路面線ｔｒｏａｄを初期値ｉｎｉ＿ｔｒｏａｄに設定し（Ｓ１０８）、後述するステップＳ１０４へ進む。 When the number n of the extracted distance information is less than the road threshold Kn (S102 / NO), the road line t road set to an initial value ini_t road (S108), the process proceeds to step S104 to be described later.
　抽出した第一距離情報Ｒ１ｉの個数ｎが路面閾値Ｋｎ以上の場合（Ｓ１０２／ＹＥＳ）、路面線演算器４０は、抽出された距離情報を線形回帰によって近似した路面線ｔｒｏａｄを算出する（Ｓ１０３）。線形回帰処理として例えば１次の最小二乗法により直線近似してもよいし、二本以上の直線近似、また曲線近似を用いてもよい。後述する車止め線の算出における線形回帰も同様である。路面線演算器４０は、算出した路面線ｔｒｏａｄを車止め位置演算器４１に出力する。 When the number n of the extracted first distance information R1i is equal to or larger than the road surface threshold Kn (S102 / YES), the road surface calculator 40 calculates a road surface road that approximates the extracted distance information by linear regression (S103). ). As linear regression processing, for example, linear approximation may be performed by a first-order least square method, or two or more linear approximations or curve approximations may be used. The same applies to linear regression in the calculation of the vehicle stop line described later. The road surface calculator 40 outputs the calculated road surface road to the vehicle stop position calculator 41.
　車止め位置演算器４１は、第一距離計測装置２０１が計測した第一距離情報Ｒ１ｉのうち、路面線ｔｒｏａｄからの距離が第二閾値Ｋ２以上であり、かつ車止め位置演算器４１が推定した車止め線ｔｂｅｒｍ＿ｏｌｄとの距離が第三閾値Ｋ３以下である第一距離情報Ｒ１ｉを抽出する（Ｓ１０４）。 The vehicle stop position calculator 41 includes a first stop information R1i measured by the first distance measurement device 201. The vehicle stop position calculator 41 estimates that the distance from the road surface road is equal to or greater than the second threshold K2 and the vehicle stop position calculator 41 estimates. First distance information R1i whose distance from the line t berm_old is equal to or smaller than the third threshold value K3 is extracted (S104).
　車止め位置演算器４１は、抽出した第一距離情報Ｒ１ｉの数が車止め閾値Ｋｍ未満である場合（Ｓ１０５／Ｎｏ）、処理を終了する。 If the number of the extracted first distance information R1i is less than the vehicle stop threshold value Km (S105 / No), the vehicle stop position calculator 41 ends the process.
　一方、車止め位置演算器４１は、抽出した第一距離情報Ｒ１ｉの数が車止め閾値Ｋｍ以上である場合（Ｓ１０５／ＹＥＳ）、抽出された第一距離情報Ｒ１ｉを線形回帰によって近似した車止め線ｔｂｅｒｍを算出する（Ｓ１０６）。 On the other hand, when the number of the extracted first distance information R1i is equal to or greater than the vehicle stop threshold value Km (S105 / YES), the vehicle stop position calculator 41 is a vehicle stop line t berm that approximates the extracted first distance information R1i by linear regression. Is calculated (S106).
　そして車止め位置演算器４１は、ｔｒｏａｄ及びｔｂｅｒｍの交点から車止めまでの距離Ｌｂ１を求めると共に、ｔｒｏａｄ及びｔｂｅｒｍの傾きから車止め傾斜角度Ａｂを算出する（Ｓ１０７）。 The bollard position calculator 41, the seek distance Lb1 from the intersection of t road and t berm until bollard, calculates the wheel stopper inclination angle Ab from the slope of t road and t berm (S107).
　次に図７及び図８を参照して、車止め幅方向形状推定部１０２の動作について説明する。図７は、車止め幅方向形状推定部による車止め幅方向形状検出処理を示すフローチャートである。図８は、車止め幅方向形状検出処理におけるマッピングを説明するための模式図である。 Next, the operation of the vehicle stop width direction shape estimation unit 102 will be described with reference to FIGS. FIG. 7 is a flowchart showing a vehicle stop width direction shape detection process by the vehicle stop width direction shape estimation unit. FIG. 8 is a schematic diagram for explaining mapping in the vehicle stop width direction shape detection process.
　ピッチ角補正器５０は、第二距離計測装置２０２の設置俯角α（図２参照）と、路面線演算器４０が推定した路面及び車体水平面Ｈの傾斜角θとを次式（１）に適用し、第二距離情報Ｒ２ｉに対してピッチ振動を補正しつつ３次元変換を行う（Ｓ２０１）。
Figure JPOXMLDOC01-appb-M000001
The pitch angle corrector 50 applies the installation depression angle α (see FIG. 2) of the second distance measuring device 202 and the inclination angle θ of the road surface and the vehicle body horizontal plane H estimated by the road surface calculator 40 to the following equation (1). Then, three-dimensional conversion is performed on the second distance information R2i while correcting the pitch vibration (S201).
Figure JPOXMLDOC01-appb-M000001
　次に車止め幅方向形状演算器５１は、自己位置推定装置３００が推定した自己位置に基づいて、ピッチ角補正器５０が補正した第二距離情報Ｒ２ｉのマッピングを行う（Ｓ２０２）。そして車止め幅方向形状演算器５１は、車止め幅方向形状記憶器５２にマッピングした第二距離情報Ｒ２ｉからなる幅方向形状情報を書込むことで、マッピングした距離情報が蓄積される（Ｓ２０３）。図８において点群Ｒ２＿ｃは現在の距離情報を示し、点群Ｒ２＿ｐｒｅ１、点群Ｒ２＿ｐｒｅ２は過去の幅方向形状情報を示す。なお、点群Ｒ２＿ｐｒｅ１よりも、点群Ｒ２＿ｐｒｅ２の方が更に前の幅方向形状情報である。スキャナー２００は、第一スキャン面２１がダンプトラック１の車体水平面に対して垂直かつ車体長手方向に平行であり、第二スキャン面２２は車体水平面に対して俯角を有し、かつ第二スキャン面２２とダンプトラック１の車体水平面との交線がダンプトラック１に備えられた後輪車軸の平行であるようにダンプトラック１に設置される。これにより、車止め幅方向形状演算器５１に入力される幅形状情報は、第二スキャン面２２と車体水平面との交線が後輪車軸（幅方向）に対して平行である場合、車体幅方向に沿って均等に得られるので望ましい。 Next, the vehicle stop width direction shape calculator 51 performs mapping of the second distance information R2i corrected by the pitch angle corrector 50 based on the self-position estimated by the self-position estimation device 300 (S202). Then, the vehicle stop width direction shape calculator 51 writes the width direction shape information composed of the second distance information R2i mapped in the vehicle stop width direction shape memory 52, thereby storing the mapped distance information (S203). In FIG. 8, a point group R2_c indicates current distance information, and a point group R2_pre1 and a point group R2_pre2 indicate past width direction shape information. It should be noted that the point group R2_pre2 is the width direction shape information before the point group R2_pre1. In the scanner 200, the first scan surface 21 is perpendicular to the vehicle body horizontal surface of the dump truck 1 and parallel to the vehicle body longitudinal direction, the second scan surface 22 has a depression angle with respect to the vehicle body horizontal surface, and the second scan surface. It is installed in the dump truck 1 so that the intersection line of 22 and the vehicle body horizontal surface of the dump truck 1 is parallel to the rear axle of the dump truck 1. As a result, the width shape information input to the vehicle stop width direction shape calculator 51 is obtained when the intersection line between the second scan surface 22 and the vehicle body horizontal plane is parallel to the rear wheel axle (width direction). This is desirable because it can be obtained evenly.
　次に図９Ａ、図９Ｂを参照してピッチ振動が発生している場合の車止め検出装置１０による車止め検出について説明する。図９Ａは、ダンプトラック１にピッチ振動が発生している場合の車止め検出処理を説明するための模式図である。図９Ｂ、図９Ｃは図９Ａの状態において第一距離計測装置が行った計測結果を示す図である。 Next, with reference to FIGS. 9A and 9B, the vehicle stop detection by the vehicle stop detection device 10 when pitch vibration is occurring will be described. FIG. 9A is a schematic diagram for explaining a vehicle stop detection process when pitch vibration is generated in the dump truck 1. 9B and 9C are diagrams showing measurement results performed by the first distance measuring device in the state of FIG. 9A.
　図９Ｂは路面が平面な場合を示す図である。第一距離計測装置２０１の鉛直方向真下の地面を中心として、路面が傾斜角θ＝０、即ち路面が水平な場合を示す。第一距離計測装置２０１からの生データは距離Ｌとレーザ照射角度φの極座標であり、これを直交座標（ｘ、ｙ）に座標変換する。そして点群のうち、路面と推定されるものを抽出し、線形解析（最小二乗法）による直線近似を行い路面直線ｙ＝ａｘ＋ｂを算出する。このとき、傾斜角θは［数２］θ＝ａｔａｎ（ａ）・・・（２）となる。 FIG. 9B is a diagram showing a case where the road surface is flat. The case where the road surface has an inclination angle θ = 0, that is, the road surface is horizontal, with the ground directly below the vertical direction of the first distance measuring device 201 as the center is shown. The raw data from the first distance measuring device 201 is polar coordinates of the distance L and the laser irradiation angle φ, and these are coordinate-converted into orthogonal coordinates (x, y). Then, from the point group, what is estimated to be a road surface is extracted, and a straight line approximation by a linear analysis (least square method) is performed to calculate a road surface line y = ax + b. At this time, the inclination angle θ is [Equation 2] θ = atan (a) (2).
　路面が理想的な平面な場合、路面直線はｙ＝ｂであり、傾斜角θ＝０となる。 When the road surface is an ideal plane, the road surface straight line is y = b and the inclination angle θ = 0.
　図９Ｃは、路面が第一距離計測装置２０１の鉛直方向真下の地面を中心として、傾斜角θ分回転した状態を示す。また、第二距離計測装置２０２が計測する場所は、ピッチ振動がない場合を基準として第二距離計測装置２０２を中心としてθ回転した場所となる。 FIG. 9C shows a state where the road surface is rotated by an inclination angle θ around the ground directly below the first distance measuring device 201 in the vertical direction. The place measured by the second distance measuring device 202 is a place rotated by θ around the second distance measuring device 202 with reference to a case where there is no pitch vibration.
　傾斜角θが発生しているとき、レーザ照射角度φは変わらないが距離Ｌは短く（または長く）なる。第一距離計測装置２０１からの生データの距離Ｌとレーザ照射角度φの極座標を直交座標（ｘ、ｙ）に変換すると、図９Ｃのように直交座標上にプロットされるｘｙ座標に変換される。図９Ｂと同様に直線近似を行い、路面直線ｙ＝ａｘ＋ｂを算出することで、傾斜角θを算出することができる。 When the tilt angle θ is generated, the laser irradiation angle φ is not changed, but the distance L is short (or long). When the polar coordinates of the distance L of the raw data from the first distance measuring device 201 and the laser irradiation angle φ are converted into orthogonal coordinates (x, y), they are converted into xy coordinates plotted on the orthogonal coordinates as shown in FIG. 9C. . The inclination angle θ can be calculated by performing linear approximation in the same manner as in FIG. 9B and calculating the road surface line y = ax + b.
　図９Ｄは、２本以上の直線近似の場合における傾斜角θを示す図である。２本以上の直線が求まった場合には、最初の直線、即ち、ダンプトラック１に近い側の直線の傾きａが傾斜角θとなる。 FIG. 9D is a diagram showing the inclination angle θ in the case of two or more linear approximations. When two or more straight lines are obtained, the inclination a of the first straight line, that is, the straight line closer to the dump truck 1 is the inclination angle θ.
　図９Ｅは、２本以上曲線近似の場合における傾斜角θを示す図である。２本以上の曲線が求まった場合には、水平方向位置が０付近、すなわちダンプトラック１に近い位置での曲線の微分値の傾きが傾斜角θとなる。 FIG. 9E is a diagram showing the inclination angle θ in the case of approximating two or more curves. When two or more curves are obtained, the slope of the differential value of the curve at the position in the horizontal direction near 0, that is, the position close to the dump truck 1 is the inclination angle θ.
　図９Ｄ、図９Ｅのいずれの場合でも、直線近似、曲線近似のいずれも、近似路面線の水平方向位置が０付近（ダンプトラック１付近）における、水平方向位置軸と路面との傾きをθとしていることが共通している。 9D and 9E, in both the linear approximation and the curve approximation, the inclination between the horizontal position axis and the road surface when the horizontal position of the approximate road surface is near 0 (near the dump truck 1) is θ. Is common.
　路面線演算器４０は、前回推定した路面線ｔｒｏａｄ＿ｏｌｄを用いて路面を計測した第一距離情報Ｒ１ｉの抽出を行い路面を推定するので、ピッチ振動が発生している状況でもピッチ振動の影響を緩和して路面の傾きを推定することができる。また、車止め位置演算器４１も前回推定した車止め線ｔｂｅｒｍ＿ｏｌｄを用いて車止めを計測した第一距離情報Ｒ１ｉの抽出を行い、車止めを推定することで、ピッチ振動が発生している状況でもピッチ振動の影響を緩和して車止めの位置及び傾斜を推定することができる。 Since the road surface calculator 40 extracts the first distance information R1i obtained by measuring the road surface using the previously estimated road surface road_old_old to estimate the road surface, the influence of the pitch vibration is affected even in the situation where the pitch vibration is occurring. It is possible to relax and estimate the slope of the road surface. The vehicle stop position calculator 41 also extracts the first distance information R1i obtained by measuring the vehicle stop using the previously estimated vehicle stop line t berm_old and estimates the vehicle stop, so that the pitch vibration is generated even in the situation where the pitch vibration is generated. The position and inclination of the vehicle stop can be estimated by mitigating the influence of the vehicle.
　ピッチ角補正器５０は、式（１）の通り路面線演算器４０が算出した車体水平面Ｈと路面の傾斜角θを用いて補正を行っているので、第二距離計測装置２０２のピッチ振動による俯角ずれを補正することができる。 Since the pitch angle corrector 50 performs correction using the vehicle body horizontal plane H calculated by the road surface computing unit 40 and the road surface inclination angle θ as expressed by the equation (1), the pitch angle corrector 50 is caused by the pitch vibration of the second distance measuring device 202. The depression angle can be corrected.
　次に図１０を参照して走行路面１００１Ａに傾きがある場合の車止め検出装置１０の動作について説明する。図１０はダンプトラック１が傾斜を持つ走行路面１００１Ａ上を走行している場合の後方検出処理を説明するための模式図である。 Next, the operation of the vehicle stop detection device 10 when the traveling road surface 1001A is inclined will be described with reference to FIG. FIG. 10 is a schematic diagram for explaining the backward detection process when the dump truck 1 is traveling on an inclined traveling road surface 1001A.
　図１０において、ダンプトラック１にピッチ振動がない場合、第一距離計測装置２０１及び第二距離計測装置２０２が計測する距離情報は、図５Ａで示した傾きのない走行路面１００１上で計測したものと同じになる。従って、傾斜を持つ走行路面１００１Ａを走行していても正確に車止め１００２までの距離と車止め角度、及び車止め形状を推定することができる。 In FIG. 10, when there is no pitch vibration in the dump truck 1, the distance information measured by the first distance measuring device 201 and the second distance measuring device 202 is measured on the traveling road surface 1001 having no inclination shown in FIG. 5A. Will be the same. Accordingly, the distance to the car stop 1002, the car stop angle, and the car stop shape can be accurately estimated even when the vehicle is traveling on the inclined road surface 1001A.
　ピッチ振動を傾斜センサ等によって計測する場合、傾斜センサは重力によって傾きを判定するため、図１０で示したような傾斜を持つ走行路面１００１Ａ上を走行する場合は、ピッチ振動がない場合でもピッチ振動があると判定されてしまい、正確なピッチ振動補正が行えない。 When the pitch vibration is measured by an inclination sensor or the like, the inclination sensor determines the inclination by gravity. Therefore, when traveling on the traveling road surface 1001A having the inclination as shown in FIG. As a result, it is determined that there is noise, and accurate pitch vibration correction cannot be performed.
　一方で、ピッチ角補正器５０はダンプトラック１の車体水平面Ｈと走行路面１００１Ａとの相対的な傾斜角を利用するため、路面の傾きによらずピッチ振動を補正することができる。従って、傾斜が変化する走行路面上を走行している場合は、ピッチ角補正器５０はピッチ振動だけでなく、路面の傾きの変化を補正することができる。 On the other hand, since the pitch angle corrector 50 uses the relative inclination angle between the vehicle body horizontal surface H of the dump truck 1 and the traveling road surface 1001A, the pitch vibration can be corrected regardless of the inclination of the road surface. Therefore, when the vehicle is traveling on a traveling road surface where the inclination changes, the pitch angle corrector 50 can correct not only the pitch vibration but also the change in the road surface inclination.
　以上説明したように、第一距離計測装置２０１及び第二距離計測装置２０２のスキャン面を交差させてダンプトラック１に設置することで、ダンプトラック１にピッチ振動が発生しているときや走行路面に傾斜がある場合でもピッチ振動による影響を緩和して車止めまでの距離と車止めの形状、向き（車止めの傾斜角や幅方向の形状）を計測することができる。これにより、放土時に適切な放土位置に停車することができる。例えば、車止めの幅方向形状にダンプトラックの車幅方向が沿うようにダンプトラックを車体の向きを誘導する際の判断情報を得ることができる。 As described above, the scanning surfaces of the first distance measuring device 201 and the second distance measuring device 202 are crossed and installed on the dump truck 1 so that when the pitch vibration is generated in the dump truck 1 or on the road surface Even when there is an inclination, it is possible to reduce the influence of pitch vibration and measure the distance to the car stop, the shape and direction of the car stop (the inclination angle of the car stop and the shape in the width direction). Thereby, it can stop at an appropriate earthing position at the time of earthing. For example, it is possible to obtain determination information for guiding the direction of the vehicle body of the dump truck so that the vehicle width direction of the dump truck follows the width direction shape of the vehicle stop.
　上記実施形態は本発明を限定するものではなく、本発明の趣旨を逸脱しない範囲での変更態様は本発明に含まれる。例えば作業機械はダンプトラックに限らず、ドーザー、ホイールローダ、油圧ショベルでもよく、これらの作業機械の後方にスキャナーを取り付けて後方障害物の形状検出を行ってもよい。また、後方障害物に限らず、スキャナーを作業機械の前、左、右に設置し、前方障害物、左障害物、右障害物の形状検出を行ってもよい。 The above embodiments do not limit the present invention, and modifications within a range not departing from the gist of the present invention are included in the present invention. For example, the work machine is not limited to a dump truck, and may be a dozer, a wheel loader, or a hydraulic excavator. A scanner may be attached to the rear of these work machines to detect the shape of a rear obstacle. Further, not only the rear obstacle but also a scanner may be installed in front, left, and right of the work machine to detect the shape of the front obstacle, the left obstacle, and the right obstacle.
　１：ダンプトラック、１０：車止め検出装置、２１：第一スキャン面、２２：第二スキャン面、２００：スキャナー、２０１：第一距離計測装置、２０２：第二距離計測装置 1: dump truck, 10: car stop detection device, 21: first scan surface, 22: second scan surface, 200: scanner, 201: first distance measurement device, 202: second distance measurement device

Claims (6)

  1. 　障害物を検出するスキャナーであって、
    　前記スキャナーは、第一距離計測装置及び第二距離計測装置を含み、
    　前記第一距離計測装置及び前記第二距離計測装置の其々は、レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定するレーザレンジスキャナにより構成され、
    　前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面及び前記第二距離計測装置のレーザ照射面を直交させて配置される、
    　ことを特徴とするスキャナー。     A scanner for detecting obstacles,
    The scanner includes a first distance measuring device and a second distance measuring device,
    Each of the first distance measuring device and the second distance measuring device is a laser range scanner that irradiates a laser, receives reflected light reflected by the detected object, and measures the distance to the detected object. Configured,
    The first distance measuring device and the second distance measuring device are arranged with the laser irradiation surface of the first distance measuring device and the laser irradiation surface of the second distance measuring device orthogonal to each other,
    A scanner characterized by that.
  2. 　障害物を検出するスキャナーを搭載した作業機械であって、
    　前記スキャナーは、
    　第一距離計測装置及び第二距離計測装置を含み、
    　前記第一距離計測装置及び前記第二距離計測装置の其々は、レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定するレーザレンジスキャナにより構成され、
    　前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面からなる第一スキャン面及び前記第二距離計測装置のレーザ照射面からなる第二スキャン面を直交させて配置され、
    　前記スキャナーは、前記作業機械の車体前後軸及び車体左右軸を含む車体水平面に対して前記第一スキャン面が垂直、かつ前記車体水平面に対して前記第二スキャン面が俯角を有して前記作業機械に設置される、
    　ことを特徴とする作業機械。     A work machine equipped with a scanner that detects obstacles,
    The scanner
    Including a first distance measuring device and a second distance measuring device;
    Each of the first distance measuring device and the second distance measuring device is a laser range scanner that irradiates a laser, receives reflected light reflected by the detected object, and measures the distance to the detected object. Configured,
    The first distance measurement device and the second distance measurement device are orthogonal to a first scan surface formed by a laser irradiation surface of the first distance measurement device and a second scan surface formed by a laser irradiation surface of the second distance measurement device. Arranged,
    The scanner is configured such that the first scan plane is perpendicular to a vehicle body horizontal plane including a vehicle body longitudinal axis and a vehicle body left and right axis of the work machine, and the second scan surface has a depression angle with respect to the vehicle body horizontal plane. Installed in the machine,
    A working machine characterized by that.
  3. 　請求項２に記載の作業機械において、
    　前記スキャナーは、前記第一スキャン面が前記作業機械の車体長手方向に平行であり、前記第二スキャン面と前記車体水平面との交線が前記作業機械に備えられた後輪車軸と平行であるように前記作業機械に設置される、
    　ことを特徴とする作業機械。     The work machine according to claim 2,
    In the scanner, the first scan surface is parallel to a longitudinal direction of the vehicle body of the work machine, and an intersection line of the second scan surface and the vehicle body horizontal surface is parallel to a rear wheel axle provided in the work machine. Installed in the work machine as
    A working machine characterized by that.
  4. 　請求項２に記載の作業機械において、
    　前記作業機械は前輪及び後輪と、前記前輪及び前記後輪の上に搭載された車体フレームと、前記車体フレームの上に搭載されたベッセルとを含むダンプトラックであって、
    　前記スキャナーは、前記ダンプトラックが走行する走行路面と前記第二スキャン面とが交わる交線と、前記後輪の最後端を前記走行路面に投影した点との距離が、前記ダンプトラックの最大後退速度の制動距離以上であるように前記作業機械に設置される、
    　ことを特徴とする作業機械。     The work machine according to claim 2,
    The work machine is a dump truck including a front wheel and a rear wheel, a body frame mounted on the front wheel and the rear wheel, and a vessel mounted on the body frame,
    The scanner is configured such that a distance between an intersection line where the traveling road surface on which the dump truck travels and the second scan surface intersect and a point where the rear end of the rear wheel is projected on the traveling road surface is the maximum backward movement of the dump truck Installed in the work machine to be greater than or equal to the braking distance of speed,
    A working machine characterized by that.
  5. 　請求項４に記載の作業機械であって、
    　前記スキャナーは、前記第二スキャン面が前記走行路面と交わる交線の長さは、前記ダンプトラックの車輪幅以上であるように設置される、
    　ことを特徴とする作業機械。     The work machine according to claim 4,
    The scanner is installed such that the length of the intersection line where the second scan surface intersects the traveling road surface is equal to or greater than the wheel width of the dump truck.
    A working machine characterized by that.
  6. 　作業機械に搭載された障害物を検出するスキャナーからの出力を基に、前記作業機械の進行方向に位置する車止めを検出する車止め検出装置であって、
    　前記スキャナーは、第一距離計測装置及び第二距離計測装置を含み、
    　前記第一距離計測装置及び前記第二距離計測装置の其々は、レーザを照射し、被検出体で反射された反射光を受光して前記被検出体までの距離を測定するレーザレンジスキャナにより構成され、
    　前記第一距離計測装置及び前記第二距離計測装置は、前記第一距離計測装置のレーザ照射面からなる第一スキャン面及び前記第二距離計測装置のレーザ照射面からなる第二スキャン面を直交させて配置され、
    　前記スキャナーは、前記作業機械の車体前後軸及び車体左右軸を含む車体水平面に対して前記第一スキャン面が垂直、かつ前記車体水平面に対して前記第二スキャン面が俯角を有して前記作業機械に設置され、
    　前記車止め検出装置は、前記スキャナーに接続された車止め検出コントローラにより構成され、
    　前記車止め検出コントローラは、
    　前記第一距離計測装置が出力した第一距離情報を用いて前記車体水平面に対する路面の傾斜角を推定し、
    　前記推定した路面の傾斜角を用いて前記第二距離計測装置が出力した第二距離情報に含まれる前記路面の傾斜角からなるピッチ角を補正し、
    　前記補正されたピッチ角を含む第二距離情報に基づいて前記作業機械の進行方向に位置する車止めの幅方向の形状を推定する、
    　ことを特徴とする車止め検出装置。     A vehicle stop detection device that detects a vehicle stop located in a traveling direction of the work machine based on an output from a scanner that detects an obstacle mounted on the work machine,
    The scanner includes a first distance measuring device and a second distance measuring device,
    Each of the first distance measuring device and the second distance measuring device is a laser range scanner that irradiates a laser, receives reflected light reflected by the detected object, and measures the distance to the detected object. Configured,
    The first distance measurement device and the second distance measurement device are orthogonal to a first scan surface formed by a laser irradiation surface of the first distance measurement device and a second scan surface formed by a laser irradiation surface of the second distance measurement device. Arranged,
    The scanner is configured such that the first scan plane is perpendicular to a vehicle body horizontal plane including a vehicle body longitudinal axis and a vehicle body left and right axis of the work machine, and the second scan surface has a depression angle with respect to the vehicle body horizontal plane. Installed on the machine,
    The vehicle stop detection device is constituted by a vehicle stop detection controller connected to the scanner,
    The vehicle stop detection controller is
    Estimating the inclination angle of the road surface with respect to the horizontal surface of the vehicle body using the first distance information output by the first distance measuring device,
    Correcting the pitch angle composed of the inclination angle of the road surface included in the second distance information output by the second distance measuring device using the estimated inclination angle of the road surface,
    Estimating the shape in the width direction of the vehicle stop located in the traveling direction of the work machine based on the second distance information including the corrected pitch angle;
    A vehicle stop detection device characterized by that.
PCT/JP2018/008859 2017-03-10 2018-03-07 Scanner, working machine, and wheel stopper detecting device WO2018164203A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2017046603A JP2018151217A (en) 2017-03-10 2017-03-10 Scanner, work machine, and car stop detector
JP2017-046603 2017-03-10

Publications (1)

Publication Number Publication Date
WO2018164203A1 true WO2018164203A1 (en) 2018-09-13

Family

ID=63448654

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2018/008859 WO2018164203A1 (en) 2017-03-10 2018-03-07 Scanner, working machine, and wheel stopper detecting device

Country Status (2)

Country Link
JP (1) JP2018151217A (en)
WO (1) WO2018164203A1 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11520009B2 (en) 2020-02-05 2022-12-06 Caterpillar Inc. Method and system for detecting an obstacle

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109185668B (en) * 2018-11-05 2020-04-14 龙岩市企信工业设计有限公司 Three-dimensional scanner fixing support for three-dimensional image acquisition of large product
JP2020134516A (en) * 2019-02-20 2020-08-31 株式会社デンソー Vehicle periphery monitoring system
JP7227871B2 (en) * 2019-08-07 2023-02-22 日立建機株式会社 material handling machine
CN111044046B (en) * 2019-12-09 2021-10-29 深圳市优必选科技股份有限公司 Method and device for testing positioning accuracy of robot
CN111824053A (en) * 2020-07-24 2020-10-27 江苏徐工工程机械研究院有限公司 Mining truck
CN113281783A (en) * 2021-05-13 2021-08-20 江苏徐工工程机械研究院有限公司 Mining truck

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009136987A (en) * 2007-12-10 2009-06-25 Toyota Motor Corp Mobile robot and method of correcting floor surface shape data
JP2009229394A (en) * 2008-03-25 2009-10-08 Ihi Corp Apparatus and method for determining traveling area of mobile robot
JP2009276195A (en) * 2008-05-14 2009-11-26 Nissan Motor Co Ltd Obstacle detector
WO2015060218A1 (en) * 2013-10-24 2015-04-30 日立建機株式会社 Reverse travel support device
US20160177524A1 (en) * 2014-12-18 2016-06-23 Federal Signal Corporation Vehicle With Internal and/or External Monitoring
JP2017015409A (en) * 2015-06-26 2017-01-19 シャープ株式会社 Road surface detection device, mobile body, road surface detection method, and road surface detection program

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009136987A (en) * 2007-12-10 2009-06-25 Toyota Motor Corp Mobile robot and method of correcting floor surface shape data
JP2009229394A (en) * 2008-03-25 2009-10-08 Ihi Corp Apparatus and method for determining traveling area of mobile robot
JP2009276195A (en) * 2008-05-14 2009-11-26 Nissan Motor Co Ltd Obstacle detector
WO2015060218A1 (en) * 2013-10-24 2015-04-30 日立建機株式会社 Reverse travel support device
US20160177524A1 (en) * 2014-12-18 2016-06-23 Federal Signal Corporation Vehicle With Internal and/or External Monitoring
JP2017015409A (en) * 2015-06-26 2017-01-19 シャープ株式会社 Road surface detection device, mobile body, road surface detection method, and road surface detection program

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11520009B2 (en) 2020-02-05 2022-12-06 Caterpillar Inc. Method and system for detecting an obstacle

Also Published As

Publication number Publication date
JP2018151217A (en) 2018-09-27

Similar Documents

Publication Publication Date Title
WO2018164203A1 (en) Scanner, working machine, and wheel stopper detecting device
US11449066B2 (en) Driving support apparatus
US9568608B2 (en) Peripheral object detection system and haulage vehicle
CN108139755B (en) Abnormality detection device for self-position estimation device, and vehicle
WO2018221453A1 (en) Output device, control method, program, and storage medium
JP6284741B2 (en) Retreat support device
WO2017119517A1 (en) Working-machine control system, working machine, and working-machine control method
US20100053593A1 (en) Apparatus, systems, and methods for rotating a lidar device to map objects in an environment in three dimensions
US20170018188A1 (en) Road shoulder-detecting system and transportation vehicle for mining
JP7133298B2 (en) Transportation vehicle control system and transportation vehicle management method
JP5147129B2 (en) Autonomous mobile
AU2009213057A1 (en) Machine sensor calibration system
JP7138538B2 (en) Laser scanner calibration method, material handling machine
WO2016121688A1 (en) Obstacle detection device for transport vehicle
JP6757676B2 (en) Transport vehicle and road surface estimation method
WO2019188015A1 (en) Working machine reversing support device
WO2019031137A1 (en) Roadside object detection device, roadside object detection method, and roadside object detection system
JP2019152580A (en) Road surface state detector
KR102545582B1 (en) System for avoiding collision in crossroad and method for control thereof
US11520009B2 (en) Method and system for detecting an obstacle
JP6553702B2 (en) Work machine control system, work machine, work machine control method and navigation controller
WO2024075577A1 (en) Mining vehicle autonomous travel system
JP2019183636A (en) Control system of work machine, work machine, control method of work machine, and navigation controller
JP7347204B2 (en) Positional relationship detection device and positional relationship detection method
JP7380196B2 (en) Positional relationship detection device

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18763185

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18763185

Country of ref document: EP

Kind code of ref document: A1