JP6909752B2 - Work machine retreat support device - Google Patents

Description

The present invention relates to a retreat support technique for supporting retreat traveling of a work machine such as a dump truck for a mine. In particular, it relates to retreat support technology at the time of soil release.

There is a technology that assists the driver of a work vehicle (transport vehicle) such as a dump truck for mining to grasp the stop position. For example, Patent Document 1 states that "the cargo of the vessel is discharged at a position where the rear end of the vehicle body or the rear end of the rear wheel tire of the transport vehicle is projected onto the road surface or a position on the road surface which is a predetermined distance rearward from the projected position. Set the reference line for soil removal work or the distance guide point for soil removal work, which is the standard for soil removal work, and display the distance guideline for soil removal work or the distance guide point for soil removal work on the display unit. The rear view display system of the transport vehicle is disclosed.

Japanese Patent No. 5380735

However, in the rear view display system of the transport vehicle disclosed in Patent Document 1, the operator sets a guideline for the distance during soil removal work as a guide when the vehicle is retracted to the soil discharge position on the image. Therefore, it is not always possible to set this reference line accurately while the transport vehicle is reversing. In particular, in an environment such as a mine where the shape of the road surface and the bollard changes frequently, it is difficult to accurately set the distance guideline for soil removal work, and as a result, it is difficult to guide the vehicle to an appropriate position.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a technique capable of guiding a transport vehicle so that the vehicle can be stopped at an appropriate discharge position at the time of discharge.

The present invention is a reverse movement support device for a work machine that supports the backward travel of the work machine, irradiates a laser, receives reflected light reflected by the detected object, and measures the distance to the detected object. The first distance measuring device and the second distance measuring device include the first distance measuring device and the second distance measuring device, and the first scanning surface and the second distance including the laser irradiation surface of the first distance measuring device. The second scan surface formed by the laser irradiation surface of the measuring device is orthogonal to the vehicle body horizontal plane including the front-rear axis and the left-right axis of the vehicle body of the work machine, and the first scan surface is perpendicular to the vehicle body horizontal plane and said. A measuring device having a depression angle on the second scanning surface and arranged on the work machine, a self-position estimation device that calculates the position and posture from the time when the work machine starts operating as a self-position, and the first Based on the road surface estimation device that calculates the inclination angle, which is the inclination of the traveling road surface on which the work machine travels with respect to the horizontal plane of the vehicle body, based on the first distance measured by the distance measuring device, and based on the first distance and the inclination angle. A vehicle stop estimation device that calculates a vehicle stop distance that is a distance to a vehicle stop located in the traveling direction of the work machine and a vehicle stop inclination angle that is an inclination angle of the vehicle stop, and a second distance measuring device that uses the inclination angle. Based on the pitch vibration correction device that performs correction processing that removes the influence of pitch vibration from the second distance measured in step 2, the second distance after the correction processing, and the self-position calculated by the self-position estimation device. A vehicle stop shape estimation device that estimates the terrain in the traveling direction of the work machine as a vehicle stop shape that is the shape of the vehicle stop, a vehicle stop shape storage device that accumulates the vehicle stop shape, the vehicle stop distance, the vehicle stop inclination angle, and the vehicle stop shape. based on the accumulated the bollard shape memory device, wherein the work machine you characterized in that and a wheel stopping inductive device which calculates the distance to the contact with the bollard.

According to the present invention, the transport vehicle can be guided so that the vehicle can be stopped at an appropriate soil discharge position at the time of soil discharge. Issues, configurations and effects other than those described above will be clarified by the description of the following embodiments.

Explanatory drawing which shows the surrounding environment at the time of unearthing of the dump truck which concerns on embodiment of this invention. The left side view of the dump truck which concerns on embodiment of this invention. The rear perspective view of the dump truck which concerns on embodiment of this invention. The block diagram which shows the functional structure of the retreat support device which concerns on embodiment of this invention. The block diagram which shows the functional structure of the contact distance calculation apparatus which concerns on embodiment of this invention. The figure which shows the image example displayed on the image display device of embodiment of this invention. The figure which shows the other image example displayed on the image display device of embodiment of this invention. (A) is a schematic diagram for explaining the vehicle stop detection process according to the embodiment of the present invention, and (b) is a schematic diagram (dump) plotting the measurement results of the first distance measuring device according to the embodiment of the present invention. Left side view of the truck). The flowchart of the car stop position estimation process which concerns on embodiment of this invention. (A) is a flowchart of the bollard width direction shape detection process according to the embodiment of the present invention, and (b) is an explanatory view of mapping in the bollard width direction shape detection process according to the embodiment of the present invention (top view of the dump truck). ). The flowchart of the contact distance determination process which concerns on embodiment of this invention. The schematic diagram (left side view of the dump truck) for demonstrating the bollard detection process when pitch vibration is generated in the dump truck which concerns on embodiment of this invention. (A) is a diagram showing a case where the road surface according to the embodiment of the present invention is a flat surface, and (b) is a diagram in which the road surface is rotated by an inclination angle θ about the ground directly below the vertical direction of the first distance measuring device. The figure which shows the state. (A) is a diagram showing an inclination angle θ in the case of two or more linear approximations according to the embodiment of the present invention, and (b) is a case of two or more curve approximations according to the embodiment of the present invention. The figure which shows the inclination angle θ in. The schematic diagram for demonstrating the rearward detection processing when the dump truck which concerns on embodiment of this invention is traveling on the traveling road surface having an inclination (left side view of the dump truck). The schematic diagram (left side view of the dump truck) for demonstrating the rearward detection processing when the dump truck which concerns on embodiment of this invention is traveling on the traveling road surface where the inclination changes.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In all the drawings for explaining the embodiment, members having the same function are designated by the same or related reference numerals, and the repeated description thereof will be omitted.

The schematic configuration of the dump truck will be described with reference to FIGS. 1 to 3. FIG. 1 is an explanatory diagram showing the surrounding environment at the time of unearthing the dump truck 1 according to the present embodiment. 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, when the dump truck 1 releases the soil to the lumber yard 1003 under the cliff, the dump truck 1 travels backward on the traveling road surface 1001 toward the bollard (Bund) 1002 arranged near the cliff. The dump truck 1 includes a right front wheel 3r and a left front wheel 3l (see FIG. 2) in the front lower portion of the vehicle body frame 2 (see FIG. 2), and a right rear wheel 4r and a left rear wheel 4l in the rear lower portion. Further, a rear axle 7 is provided at the rear of the vehicle body frame 2. On the other hand, a driver's seat 5 (see FIG. 2) is provided on the front upper portion of the vehicle body frame 2, and a vessel 6 is provided on the rear upper portion.

Further, the dump truck 1 includes a backward support device 100 that supports the backward travel of the dump truck 1. The backward support device 100 includes a photographing device (camera) 102 that photographs the rear of the dump truck 1, a contact distance calculating device 101 that calculates the distance (contact distance) to the car stop, and a display image generating device 103 (see FIG. 4). The image display device (monitor) 104 (see FIG. 4), the release location approach determination device 105 (see FIG. 4), and the release location approach notification device (buzzer, lamp) 106 (see FIG. 4). include.

The image display device 104 and the soil release location approach notification device 106 are arranged in the driver's seat 5.

The contact distance calculation device 101 scans the rear of the dump truck 1 to accumulate the shape of the bollard, and calculates the distance (contact distance) until the dump truck 1 comes into contact with the bollard. The contact distance calculation device 101 includes a measurement device (scanner) 200 including a plurality of laser scanners, a self-position estimation device 300 for detecting the self-position of the dump track 1, and a sensor output and self-position estimation device 300 from the measurement device 200. It includes a bollard detection device 10 that detects a bollard based on its own position from the vehicle, and a bollard guidance device 60 (see FIG. 5) that calculates information for guiding the dump truck 1 to the detected bollard.

In the following description, the position of the dump truck 1 when the backward support device 100 starts operating is set as the origin, the traveling direction at the moment when the backward support device 100 starts operating is set as the y-axis, and the vehicle body height of the dump truck 1 is set. A Cartesian coordinate system 400 is used in which the axis is the z-axis and the axis orthogonal to the two axes of the y-axis and the z-axis is the x-axis.

The measuring device 200 includes a first distance measuring device 201 and a second distance measuring device 202. The measuring device 200 is installed behind the vehicle body of the dump truck 1, more specifically above the rear axle 7 and below the vessel 6, between the left rear wheel 4l and the right rear wheel 4r. The first distance measuring device 201 and the second distance measuring device 202 do not have to be arranged at the same height and do not have to 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 the reflected light from the detected object, and detects the distance and direction to the detected object. Radar). The first distance measuring device 201 and the second distance measuring device 202 include a laser irradiation surface (hereinafter referred to as “second scanning surface”) 22 of the second distance measuring device 202 and a laser irradiation surface (hereinafter referred to as “second scanning surface”) of the first distance measuring device 201. It is arranged so as to be orthogonal to (referred to as the first scan plane) 21. Note that the orthogonality referred to here is not limited to the case where the normal vectors of the first scan surface 21 and the second scan surface 22 are strictly vertical, and is the same 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 scanning 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 scan surface 21 extends from at least vertically downward to the vehicle body horizontal plane H including the front-rear axis and the left-right axis of the vehicle body frame 2 from downward 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 sets the traveling road surface 1001 at the position P1 where the second scanning surface 22 and the traveling road surface 1001 intersect, and the rearmost ends of the left rear wheel 4l and the right rear wheel 4r. It is installed on the dump truck 1 so as to have an installation depression angle α (FIG. 2) with respect to the horizontal plane H of the vehicle body so that the distance D1 from the position P2 projected on the above is equal to or greater than the braking distance of the maximum retreat speed of the dump truck 1. .. By installing in this way, braking is possible without contacting the bollard even when reversing at the maximum retreat speed.

Further, 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 horizontal plane H of the vehicle body, and the shielding of the dump truck 1 with respect to the second scanning 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 the area through which the wheels of the dump truck 1 pass in advance.

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 by using a plurality of laser scanners. Further, the number of the first scan surface 21 is not limited to one with respect to the first distance measuring device 201, and there may be a plurality of the first scan surfaces 21. Similarly, the number of the second scanning surface 22 is not limited to one with respect to the second distance measuring device 202, and there may be a plurality of the second scanning surfaces 22.

The photographing device 102 is, for example, a camera that acquires an image behind the dump truck 1. The photographing device 102 is a first scan behind the dump truck 1, more specifically above the measuring device 200 and below the vessel 6, between the left rear wheel 4l and the right rear wheel 4r, behind the dump truck 1. The surface 21 and the second scan surface 22 are included in the field of view and installed.

Each position in the visual field range of the dump truck 1 in the vehicle coordinate system and the pixel position are associated with each other in advance and stored in a memory such as a ROM accessible to the display image generation device 103. The vehicle coordinate system of the dump truck 1 has the center of gravity of the dump truck 1 as the origin, and the y-axis is the front-rear axis direction of the vehicle body on the horizontal plane of the vehicle body, the x-axis direction is the left-right axis direction, and the z-axis direction is the direction orthogonal to the horizontal plane of the vehicle body. It is a system.

FIG. 4 is a block diagram showing the functional configurations of the retreat support device 100 and FIG. 5 is a block diagram of the contact distance calculation device 101.

The self-position estimation device 300 is connected to each of the wheel speed measuring device (wheel speed sensor) 301 and the steering angular velocity measuring device (yaw rate sensor, turning angular velocity sensor) 302 included in the dump truck 1, and the wheels from the wheel speed measuring device 301. The rotation speed is acquired, and the measurement result of the turning angular velocity of the dump truck 1 is acquired from the steering angular velocity measuring device 302. Then, the self-position estimation device 300 calculates the speed and angular velocity of the dump truck 1 and the position and orientation of the above-mentioned Cartesian coordinate system 400 (see FIG. 1) from the origin by executing, for example, a dead reckoning process.

An IMU (Inertial Measurement Unit) may be used instead of the wheel speed measuring device 301 for the purpose of measuring the vehicle body speed, and a steering angle sensor may be used instead of the yaw rate sensor as the steering angle measuring device 302 for the purpose of estimating the turning angular velocity. good. Further, for the purpose of measuring the position and orientation of the Cartesian coordinate system 400 from the origin, a GPS (Global Positioning System) and a geomagnetic sensor may be used instead of the self-position estimation device 300.

The bollard detection device 10 includes a first bollard detection device 40 that estimates the bollard position and a second bollard detection device 50 that estimates the shape of the bollard in the width direction. Further, the car stop guidance device 60 includes a car stop guidance determination device 61 and a vehicle body shape storage device 62.

The first bollard detection device 40 includes a road surface estimation device 41 and a bollard estimation device 42. The output of the first distance measuring device 201 described above is connected to the respective inputs of the road surface estimation device 41 and the bollard estimation device 42. The output of the road surface estimation device 41 is connected to the inputs of the bollard estimation device 42 and the pitch vibration correction device 51 described later. The output of the first distance measuring device 201 is further connected to the input of the bollard estimation device 42.

The first distance information R1i output by the first distance measuring device 201 (i: indicates the laser irradiation angle φ in the first distance measuring device 201, for example −45 ≦ i ≦ 225, and the same applies to the second distance information R2i described later). Includes the measurement point and the distance to the measurement point.

The road surface estimation device 41 extracts the distance information indicating the measurement point estimated to be the road surface and the distance from the first distance information R1i, and calculates the road surface line. Then, the inclination angle θ (see FIG. 12) of the road surface line with respect to the vehicle body horizontal plane H including the front-rear axis and the left-right axis of the vehicle body frame 2 is calculated, and the inclination of the road surface with respect to the vehicle body horizontal plane H is calculated. The inclination angle θ is also referred to as a pitch angle θ.

The bollard estimation device 42 extracts the distance information of the first distance information R1i measured by the first distance measuring device 201, which is presumed to be a bollard, and calculates the intersection of the road surface and the bollard and the inclination of the bollard with respect to the road surface. ..

The second bollard detection device 50 includes a pitch vibration correction device 51, a bollard shape estimation device 52, and a bollard shape storage device 53. The input of the pitch vibration correction device 51 is connected to the output of the second distance measuring device 202 and the road surface estimation device 41, respectively. The output of the pitch vibration correction device 51 is connected to the input of the bollard shape estimation device 52. The input of the bollard shape estimation device 52 is further connected to the output of the self-position estimation device 300. The bollard shape storage device 53 is connected to the bollard shape estimation device 52 and the bollard guidance determination device 61 described later.

The pitch vibration correction device 51 removes the influence of 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 estimating device 41. Executes the correction process of.

The bollard shape estimation device 52 is a general SLAM (Simultaneus Localization and Mapping) based on the second distance information R2i corrected by the pitch vibration correction device 51 and the position and orientation of the dump truck 1 estimated by the self-position estimation device 300. The terrain in the traveling direction of the dump truck 1 is calculated as the shape of the bollard 1002 in the vehicle width direction by a mapping method such as.

The bollard shape storage device 53 stores width direction shape information indicating the shape of the bollard 1002 in the bollard direction calculated by the bollard shape estimation device 52. The bollard shape estimation device 52 may refer to the bollard shape information accumulated by the bollard shape storage device 53 in order to calculate the shape information in the width direction at a higher altitude. The stored bollard shape information is also referred to by the display image generator 103.

The bollard guidance device 60 includes a bollard guidance determination device 61 and a vehicle body shape storage device 62. The input of the bollard guidance determination device 61 is connected to the outputs of the bollard estimation device 42 and the self-position estimation device 300, and is also connected to each of the bollard shape storage device 53 and the vehicle body shape storage device 62. The bollard guidance determination device 61 reads out the information stored in the bollard shape storage device 53 and the vehicle body shape storage device 62 as needed.

The bollard guidance determination device 61 acquires the distance to the bollard estimated by the bollard estimation device 42, the inclination of the bollard, the width direction shape of the bollard stored by the bollard shape storage device 53, and the steering acquired via the self-position estimation device 300. The contact distance required for determining guidance is calculated based on the turning angular velocity measured by the angle measuring device 302 and the vehicle body shape (including the size in the vehicle width direction) stored in the vehicle body shape storage device 62. The bollard guidance determination device 61 outputs the calculated contact distance to the soil discharge site approach determination device 105.

The display image generation device 103 generates a display image by superimposing the bollard shape accumulated in the bollard shape storage device 53 on the image acquired by the photographing device 102. The input of the display image generation device 103 is connected to the respective outputs of the photographing device 102 and the contact distance calculation device 101, and the output of the display image generation device 103 is connected to the input of the image display device 104. The display image generation device 103 superimposes the bollard shape on the display image in an identifiable manner. The bollard shape represented by the value of the Cartesian coordinate system 400 is converted into a pixel position using the self-position and orientation of the dump truck 1 calculated by the self-position estimation device 300.

The image display device 104 displays the display image generated by the display image generation device 103. FIG. 6 shows an image example 410 displayed on the image display device 104. The bollard shape 1002b is displayed in an identifiable manner in the image behind the dump truck 1 including the vessel 6, the left rear wheel 4l, and the right rear wheel 4r.

When the image pickup device 102 can acquire the image around the dump truck 1, the display image generation device 103 generates a bird's-eye view image from the image acquired by the image pickup device 102, and superimposes the vehicle stop shape on the generated bird's-eye view image. To generate a display image. A display image example 420 in this case is shown in FIG. An icon image 1a representing the dump truck 1 is displayed in the center. The conversion table and icon image 1a used when converting the acquired video into a bird's-eye view image are stored in advance in a storage device such as a ROM included in the backward support device 100.

The release location entry determination device 105 determines whether or not the dump truck 1 has entered the release location from the contact distance calculated by the vehicle stop guidance determination device 61 and the release location predetermined by the distance from the vehicle stop. If it is determined that the vehicle has entered, a notification signal is output to the soil release site approach notification device 106. The input of the soil release site approach determination device 105 is connected to the output of the contact distance calculation device 101. The output of the release location approach determination device 105 is connected to the input of the release location approach notification device 106.

The soil release site approach notification device 106 includes, for example, a buzzer and a lamp. When the release location approach notification device 106 receives the notification signal from the release location approach determination device 105, the release location approach notification device 106 outputs a notification output such as outputting a sound from the buzzer or turning on the lamp.

The vehicle stop detection device 10, the vehicle stop guidance device 60, the self-position estimation device 300, the display image generation device 103, and the excavation site approach determination device 105 are microcomputer devices 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 functions of each device, or each device may be configured by an arithmetic circuit.

Next, a process of detecting a bollard behind the dump truck 1 by using the measuring device 200 will be described with reference to FIGS. 8 (a) and 8 (b). FIG. 8A is a schematic diagram for explaining a car stop detection process by the car stop detection device 10. FIG. 8B is a schematic view plotting the measurement results of the first distance measuring device 201.

The dotted line 1011 in FIG. 8A represents the distance to the object to be detected (including both the road surface and the bollard) measured by the first bollard detection device 40. Further, the black point 1012 in FIG. 8A indicates a measurement point measured by the first distance measuring device 201. The black dot 1013 in FIG. 8B indicates the position where the first distance measuring device 201 outputs the first distance information R1i on the orthogonal coordinates.

When the dump truck 1 starts to retreat toward the bollard 1002, the scanner, which is the measuring device 200, irradiates the laser at −45 ° to -225 ° while rotating, and the bollard 1002 is tilted from the rearmost end of the rear wheel of the dump truck 1. The distance to each point lined up along the surface 1002a is calculated and output as the first distance information R1i. As the dump truck 1 approaches the bollard 1002, the line of intersection between the first scan surface 21 and the road surface 1001 becomes shorter.

As shown in FIG. 8B, when the first distance information R1i is plotted in a coordinate system in which the horizontal axis indicates the horizontal position and the vertical axis indicates the vertical height, a straight line 1021 substantially parallel to the horizontal direction and a vertical height are obtained. A straight line 1022 in which the value changes is obtained. The intersection 1023 of the two straight lines is the inflection point of the first distance information R1i, and it can be estimated that the _{road surface (troad} ) and the bollard (t berm) meet.

Further, in FIG. 8A, since the dump truck 1 is sufficiently close to the bollard 1002, the second scan surface 22 calculates the distances to the points lined up in the width direction on the inclined surface 1002a instead of the traveling road surface 1001. Is output as the second distance information R2i. After that, as the dump truck 1 approaches the bollard 1002 further, the second scan surface 22 moves toward the upper end of the inclined surface 1002a.

Next, the operation of the first bollard detection device 40 will be described with reference to FIG. FIG. 9 is a flowchart showing a car stop position estimation process.

The road surface estimation device 41 extracts the first distance information estimated to be the road surface from the first distance information R1i measured by the first distance measurement device 201 (S101). _{Specifically, the first distance information R1i in which the distance from the road surface line trad_old} estimated by the road surface estimation device 41 in the previous measurement is equal to or less than the first threshold value K1 is extracted.

The road surface estimation device 41 compares the number n of the first distance information R1i extracted as the road surface with a predetermined threshold value (road surface threshold value Kn). 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 S104 to be described later.

When the number n of the extracted first distance information R1i is equal to or greater than the road surface threshold value Kn (S102 / YES), the road surface estimation device 41 _{calculates the road} surface line load from the extracted distance information (S103). _{Here, the road} surface line load is calculated by approximating the extracted first distance information R1i by linear regression. As the linear regression processing, for example, linear approximation may be performed by a first-order least squares method, two or more linear approximations, or curve approximation may be used. The same applies to the linear regression in the calculation of the bollard line, which will be described later. The road surface estimation device 41 outputs the calculated _road surface line load to the bollard estimation device 42.

The bollard estimation device 42 extracts the first distance information R1i presumed to be a bollard from the first distance information R1i measured by the first distance measuring device 201 (S104). Here, in the first distance information R1i measured by the first distance measuring device 201, _{the distance from the road} surface line trad is equal to or more than the second threshold value K2, and the bollard estimated by the bollard estimation device 42 in the previous measurement is stopped. The first distance information R1i in which the distance from the line t _{berm_old} is equal to or less than the third threshold value K3 is extracted.

The bollard estimation device 42 compares a few meters of the first distance information R1i extracted as a bollard with a predetermined threshold value (buffer stop threshold value Km). Then, when the number m of the first distance information R1i extracted as the bollard is less than the bollard threshold Km (S105 / No), the process ends.

On the other hand, the wheel stop estimating device 42, the extracted first distance information when the number m of R1i is above bollard threshold Km (S105 / YES), the first distance information R1i extracted approximated by linear regression bollard line _{t berm} Is calculated (S106).

Then, the car stop estimation device 42 obtains the distance Lb1 from the intersection of the _{road surface line trad} and the car stop line t berm to the car stop, and calculates the car stop inclination angle Ab from the inclinations of the _road and the _{berm (S107).}

Next, the operation of the second bollard detection device 50 will be described with reference to FIGS. 10 (a) and 10 (b). FIG. 10A is a flowchart showing the shape detection process in the width direction of the bollard by the second bollard detection device 50. FIG. 10B is a schematic diagram for explaining mapping in the bollard width direction shape detection process.

The pitch vibration correction device 51 has the following equation (see FIG. 12) of the installation depression angle α of the second distance measuring device 202 (see FIG. 2) and the inclination angle θ of the vehicle body horizontal plane H with respect to the road surface estimated by the road surface estimation device 41 (see FIG. 12). Applying to 1) to (3), three-dimensional conversion is performed on the second distance information R2i while correcting the pitch vibration (S201).
xi = R2i × cos (φ) ・ ・ ・ (1)
yi = R2i × sin (φ) × cos (α + θ) ・ ・ ・ (2)
zi = R2i × sin (φ) × sin (α + θ) ・ ・ ・ (3)

Next, the bollard shape estimation device 52 maps the second distance information R2i corrected by the pitch vibration correction device 51 based on the self-position estimated by the self-position estimation device 300 (S202). Then, the bollard shape estimation device 52 writes the width direction shape information including the corrected second distance information R2i mapped to the bollard shape storage device 53, and accumulates the mapped distance information (S203).

In FIG. 10B, the point group R2_c shows the current distance information, and the point group R2_pre1 and the point group R2_pre2 show the past width direction shape information. It should be noted that the point group R2_pre2 is further earlier in the width direction shape information than the point group R2_pre1.

Next, the operation of the vehicle stop guidance device 60 will be described with reference to FIG. FIG. 11 is a flowchart of the contact distance determination process by the vehicle stop guidance device 60.

The bollard guidance determination device 61 compares the distance Lb1 to the bollard and the bollard tilt angle Ab calculated by the bollard estimation device 42 with the predetermined fourth threshold value K4 and fifth threshold value K5, respectively.

When the distance Lb1 is equal to or less than the fourth threshold value K4 and the vehicle stop inclination angle Ab is equal to or greater than the fifth threshold value K5 (S301 / Yes), the vehicle stop guidance determination device 61 estimates the predicted traveling path p (S302). ). The traveling path p is estimated based on the turning angular velocity (steering angle) s measured by the steering angle measuring device 302 and acquired via the self-position estimation device 300.

Next, the bollard guidance determination device 61 is based on the traveling path p, the width direction shape information G stored in the bollard shape storage device 53 by the second bollard detection device 50, and the bollard shape D stored in the bollard shape storage device 62. The distance Lb2 to the bollard 1002 is calculated (S303).

When the calculated distance Lb2 is equal to or less than the sixth threshold value K6 (S304 / Yes), the distance Lb2 is defined as the contact distance Lc (S305).

When the calculated distance Lb is larger than the sixth threshold value K6 (S304 / No), the distance Lb1 is defined as the contact distance Lc (S306).

When the distance Lb1 is larger than the fourth threshold value K4 or the bollard inclination angle Ab is less than the fifth threshold value K5 (S301 / No), the fourth threshold value K4 is defined as the contact distance Lc.

Next, the bollard detection by the bollard detection device 10 when pitch vibration is generated will be described. FIG. 12 is a schematic diagram for explaining a car stop detection process when pitch vibration is generated in the dump truck 1. 13 (a) and 13 (b) are diagrams showing the measurement results performed by the first distance measuring device 201 in the state of FIG.

FIG. 13A is a diagram showing a case where the road surface is a flat surface. The case where the road surface has an inclination angle θ = 0, that is, the road surface is horizontal is shown centering on the ground directly below the first distance measuring device 201 in the vertical direction. The raw data from the first distance measuring device 201 is the polar coordinates of the distance L and the laser irradiation angle φ, and these are coordinate-converted into Cartesian coordinates (x, y). Then, the point cloud that is presumed to be the road surface is extracted, and a straight line approximation is performed by linear analysis (least squares method) to calculate the road surface straight line y = ax + b. At this time, the inclination angle θ is atan (a).

When the road surface is an ideal plane, the road surface straight line is y = b and the inclination angle θ = 0.

FIG. 13B shows a state in which the road surface is rotated by an inclination angle θ with respect to the ground directly below the first distance measuring device 201 in the vertical direction. Further, 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 the case where there is no pitch vibration.

When the tilt angle θ is generated, the laser irradiation angle φ does not change, but the distance L becomes shorter (or longer). When the distance L of the raw data from the first distance measuring device 201 and the polar coordinates of the laser irradiation angle φ are converted into Cartesian coordinates (x, y), the xy coordinates plotted on the Cartesian coordinates as shown in FIG. 13 (b). Is converted to. The inclination angle θ can be calculated by performing a straight line approximation in the same manner as in FIG. 13A and calculating the road surface straight line y = ax + b.

FIG. 14A is a diagram showing an 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. 14B is a diagram showing an inclination angle θ in the case of two or more curve approximations. When two or more curves are obtained, the slope of the differential value of the curves at a position near 0 in the horizontal direction, that is, at a position close to the dump truck 1, is the inclination angle θ.

In both cases of FIGS. 14 (a) and 14 (b), the horizontal position axis and the road surface when the horizontal position of the approximate road surface line is near 0 (near the dump track 1) in both the straight line approximation and the curve approximation. It is common that the slope with and is θ.

Since the road surface estimation device 41 _{extracts the first distance information R1i that measures the road surface using the previously estimated road surface line slope_old} and estimates the road surface, the influence of the pitch vibration is mitigated even in the situation where the pitch vibration is generated. The slope of the road surface can be estimated. In addition, the _{bollard estimation device 42 also extracts the first distance information R1i that measures the bollard using the previously estimated bollard line t berm_old,} and estimates the bollard so that the bollard vibration can be generated even in a situation where the bollard vibration is occurring. The influence can be mitigated and the position and inclination of the bollard can be estimated.

Since the pitch vibration correction device 51 corrects using the vehicle body horizontal plane H calculated by the road surface estimation device 41 as shown in equations (1) to (3) and the inclination angle θ of the road surface, the second distance measurement device 202 It is possible to correct the depression angle deviation due to the pitch vibration of.

Next, the operation of the bollard detection device 10 when the traveling road surface 1001A is inclined will be described with reference to FIG. FIG. 15 is a schematic diagram for explaining a rearward detection process when the dump truck 1 is traveling on a traveling road surface 1001A having an inclination.

In FIG. 15, when the dump truck 1 has no pitch vibration, the distance information measured by the first distance measuring device 201 and the second distance measuring device 202 is on the non-inclined traveling road surface 1001 shown in FIG. 8 (b). It will be the same as the measured one. Therefore, the distance to the bollard 1002, the bollard angle, and the bollard shape can be accurately estimated even when traveling on the traveling road surface 1001A having an inclination.

When the pitch vibration is measured by a tilt sensor or the like, the tilt sensor determines the tilt by gravity. Therefore, when traveling on a traveling road surface 1001A having a slope as shown in FIG. 15, the pitch vibration even if there is no pitch vibration. It is determined that there is, and accurate pitch vibration correction cannot be performed.

On the other hand, since the pitch vibration correction device 51 uses the relative inclination angle θ between the vehicle body horizontal plane 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 traveling on a traveling road surface where the inclination changes, the pitch vibration correction device 51 can correct not only the pitch vibration but also the change in the inclination of the road surface.

Next, the operation of the bollard detection device 10 when there is a change in the inclination of the traveling road surface 1001B will be described with reference to FIG. FIG. 16 is a schematic view for explaining a rearward detection process when the dump truck 1 is traveling on a traveling road surface 1001B having a change in inclination.

In FIG. 16, the inclination of the traveling road surface 1001B changes from the middle, but since the first distance measuring device 201 measures the distance in the traveling direction, the current dump truck 1 and the traveling road surface 1001B are relative to each other. Instead of measuring the slope, the relative slope between the road surface to be driven in the future and the dump truck 1 is measured.

Therefore, when traveling on the traveling road surface 1001B where the inclination changes, the pitch vibration correction device 51 can correct not only the pitch vibration but also the change in the inclination of the road surface.

As described above, by crossing the scan surfaces of the first distance measuring device 201 and the second distance measuring device 202 and installing them on the dump truck 1, when pitch vibration is generated in the dump truck 1 or the traveling road surface. Even if there is an inclination, the influence of pitch vibration can be mitigated and the distance to the bollard and the shape and orientation of the bollard (the tilt angle of the bollard and the shape in the width direction) can be measured accurately. According to this embodiment, the dump truck 1 can be guided to the bollard by using the accurately measured distance to the bollard and the shape and orientation of the bollard. As a result, it is possible to guide the vehicle to stop at an appropriate discharge position at the time of discharge.

The above-described embodiment does not limit the present invention, and modifications thereof without departing from the spirit of the present invention are included in the present invention. For example, the work machine is not limited to a dump truck, but may be a dozer, a wheel loader, or a hydraulic excavator, and a scanner may be attached to the rear of these work machines to detect the shape of a rear obstacle. Further, not limited to the rear obstacle, the 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.

Further, the display image generation device 103 and the discharge location approach determination device 105 are merely examples of the output destinations of the contact distance calculation device 101, and are other devices, for example, so-called unmanned dump trucks in which the dump truck 1 autonomously travels. In some cases, the output is output to an autonomous driving control device that controls autonomous driving, and the distance from the dump truck 1 to the vehicle stop and the orientation of the vehicle body of the dump truck 1 with respect to the vehicle stop (direction of the vehicle body horizontal plane, vehicle body front-rear axis, and vehicle body left and right). The autonomous driving control device may be configured to perform calculations necessary for stopping the vehicle, such as the orientation of the shaft). In this case, the photographing device 102, the display image generation device 103, the image display device 104, the soil release location approach determination device 105, and the soil release location approach notification device 106 may not be provided.

Further, the dump truck 1 may be provided with a contact determination device, and this contact determination device may be used as an output destination of the contact distance calculation device 101. Then, the contact determination device may determine the contact with the bollard and perform control for avoiding the contact (interference) with the bollard.

1: Dump truck, 1a: Icon image, 2: Body frame, 3l: Left front wheel, 3r: Right front wheel, 4l: Left rear wheel, 4r: Right rear wheel, 5: Driver's seat, 6: Vessel, 7: Rear Askul ,
100: Retreat support device, 101: Contact distance calculation device, 102 imaging device, 103: Display image generation device, 104: Video display device, 105: Soil release location approach determination device, 106: Soil release location approach notification device,
10: Car stop detection device, 40: First car stop detection device, 41: Road surface estimation device, 42: Car stop estimation device, 50: Second car stop detection device, 51: Pitch vibration correction device, 52: Car stop shape estimation device, 53: Car stop shape storage device, 60: Car stop guidance device, 61: Car stop guidance judgment device, 62: Body shape storage device,
200: Measuring device, 201: First distance measuring device, 202: Second distance measuring device, 21: First scanning surface, 22: Second scanning surface,
300: Self-position estimation device, 301: Wheel speed measurement device, 302: Steering angle measurement device, 400: Cartesian coordinate system,
1001: Driving road surface, 1001A: Driving road surface, 1001B: Driving road surface, 1002: Car stop, 1002a: Inclined surface, 1002b: Car stop shape, 1003: Dumping ground, 1011: Dot line, 1012: Black point, 1013: Black point, 1023: Intersection

Claims (7)

It is a backward support device for work machines that supports backward travel of work machines.
The first distance measuring device and the second distance measuring device include a first distance measuring device and a second distance measuring device that irradiate a laser and receive the reflected light reflected by the detected body to measure the distance to the detected body. In the second distance measuring device, the first scanning surface formed by the laser irradiation surface of the first distance measuring device and the second scanning surface formed of the laser irradiation surface of the second distance measuring device are orthogonal to each other, and the vehicle body of the work machine A measuring device arranged on the work machine with the first scan surface perpendicular to the horizontal plane of the vehicle body including the front-rear axis and the left-right axis and the second scan surface having a depression angle with respect to the horizontal plane of the vehicle body .
A self-position estimation device that calculates the position and posture from the start of operation of the work machine as the self-position, and
A road surface estimation device that calculates an inclination angle, which is an inclination of the traveling road surface on which the work machine travels with respect to the horizontal plane of the vehicle body, based on the first distance measured by the first distance measuring device.
Based on the first distance and the inclination angle, a vehicle stop estimation device that calculates a vehicle stop distance, which is a distance to a vehicle stop located in the traveling direction of the work machine, and a vehicle stop inclination angle, which is an inclination angle of the vehicle stop.
A pitch vibration correction device that performs correction processing that removes the influence of pitch vibration from the second distance measured by the second distance measuring device using the inclination angle.
Based on the second distance after the correction process and the self-position calculated by the self-position estimation device, the bollard shape estimation that estimates the terrain in the traveling direction of the work machine as the bollard shape, which is the shape of the bollard. Equipment and
A bollard shape storage device that stores the bollard shape and
It is provided with a bollard guidance device that calculates the distance until the work machine comes into contact with the bollard based on the bollard distance, the bollard tilt angle, and the bollard shape stored in the bollard shape storage device. A featured work machine retreat support device. The work machine retreat support device according to claim 1.
The pitch vibration correction device calculates orthogonal coordinates (xi, yi, zi) from the second distance (R2i, φ) represented by polar coordinates, where the depression angle is α and the inclination angle is θ. And the second distance after the correction process
xi = R2i × cos (φ)
yi = R2i × sin (φ) × cos (α + θ)
zi = R2i × sin (φ) × sin (α + θ)
A work machine retreat support device characterized by. The work machine retreat support device according to claim 1.
The first distance measuring device and the second distance measuring device are retreat support devices for work machines, each of which has one or more laser radar irradiation surfaces. The work machine retreat support device according to claim 1.
The depression angle of the second distance measuring device is the intersection of the traveling road surface on which the work machine travels and the second scanning surface, and the point where the rearmost end of the rear wheel of the work machine is projected onto the traveling road surface. A retreat support device for a work machine, characterized in that the distance is set to be equal to or greater than the braking distance of the maximum retreat speed of the work machine. The work machine retreat support device according to claim 1.
The work machine is characterized in that the second distance measuring device is installed so that the length of the intersection line between the second scan surface and the traveling road surface on which the work machine travels is equal to or greater than the wheel width of the work machine. Retreat support device. The work machine retreat support device according to claim 1.
An imaging device that photographs the rear of the work machine,
A display image generator that generates a display image from the image acquired by the photographing device, and
A video display device for displaying the generated display video is further provided.
The display image generation device is a backward support device for a work machine, which generates the display image by superimposing the shape of the bollard detected on the image acquired by the photographing device in an identifiable manner. The work machine retreat support device according to claim 1.
When the work machine enters the soil release site, a discharge site entry determination device that outputs a notification signal and
A retreat support device for a work machine, further comprising a soil release site approach notification device that outputs a notification when the notification signal is received.

Images