Step position detection device

Abstract

To provide a step position detection device for detecting a low position with high accuracy.SOLUTION: According to one aspect of the present invention, a step position detection device includes a point group information acquisition part for acquiring a situation including at least a road surface around a work machine as point group information, a road surface estimation part for estimating an area in which irregularities of the road surface do not exceed prescribed height as a flat road on the basis of the point group information including height information acquired by the point group information acquisition part, and an extension road surface estimation part for estimating an extension part of the road surface hidden by a step in a first flat road estimated by the road surface estimation part with the work machine existing, and a second flat road existing at a position lower than the first flat road.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present invention relates to a step position detection device. [Background technology]

[0002] In recognition processing using a stereo camera, a technology is known in which three-dimensional information of the road surface and objects is obtained from parallax images generated by parallax calculation, and a warning is given to a vehicle equipped with the camera of the possibility of an accident.Patent Document 1 discloses a technology in which straight lines are extracted using parallax images acquired from a stereo camera, and when two straight lines are extracted, it is determined that a low point exists. Summary of the Invention [Problem to be solved by the invention]

[0003] In the technology disclosed in Patent Document 1, when the shape of the low point changes or when there is a slope with a large gradient angle, it is difficult to accurately detect the low point using the parallax image acquired from the camera.

[0004] An object of the present invention is to detect low positions with high accuracy. [Means for solving the problem]

[0005] A step position detection device according to one embodiment of the present invention comprises a point cloud information acquisition unit that acquires the situation around a work machine as point cloud information, including at least the road surface; a road surface estimation unit that estimates, based on point cloud information including height information acquired by the point cloud information acquisition unit, an area where the unevenness of the road surface does not exceed a predetermined height as a flat road; and an extended road surface estimation unit that estimates the extended portions of the road surface that are hidden by steps on a first flat road on which the work machine is located, estimated by the road surface estimation unit, and a second flat road that is lower than the first flat road. [Effects of the Invention]

[0006] The step position detection device according to the present invention can detect low positions with high accuracy. [Brief explanation of the drawings]

[0007] [Figure 1] 1 is an overall view of a work machine including a step position detection device according to an embodiment of the present invention. [Figure 2] 1 is a diagram illustrating a hardware configuration of a step position detection device according to an embodiment of the present invention. [Figure 3] 1 is a functional block diagram of a step position detection device according to an embodiment of the present invention. [Figure 4] FIG. 3 is a flowchart of a process performed by a step position detection device according to an embodiment of the present invention. [Figure 5] 10A and 10B are diagrams for explaining a road surface estimation process using the attachment angle of the point cloud information acquisition unit to the work machine. [Figure 6] 1A and 1B are diagrams showing an example of a two-dimensional map created based on acquired point cloud information and a screen divided into a predetermined grid size in a step position detection device according to an embodiment of the present invention. [Figure 7] 10 is a diagram showing an example of the relationship between the actual distance and the corresponding actual height at a position in the real space of a flat road in a step position detection device according to an embodiment of the present invention. FIG. [Figure 8] FIG. 4 is a flowchart of a road surface estimation process in the step position detection device according to one embodiment of the present invention. [Figure 9] 10A and 10B are diagrams illustrating an example of step boundary surfaces for each grid in step position detection according to an embodiment of the present invention. [Figure 10] FIG. 4 is a flowchart of a step shape determination process in the step position detection device according to one embodiment of the present invention. [Figure 11] FIG. 4 is a flowchart of an extended road surface estimation process in the step position detection device according to one embodiment of the present invention. [Figure 12] FIG. 4 is a flowchart of a step detection process in the step position detection device according to one embodiment of the present invention. [Figure 13]1 is a conceptual diagram for explaining an example of detecting a step position in a step position detection device according to an embodiment of the present invention. FIG. DETAILED DESCRIPTION OF THE INVENTION

[0008] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The following describes the preferred embodiments of the present invention with reference to the accompanying drawings. In the drawings, the same components are designated by the same reference numerals, and redundant explanations may be omitted.

[0009] <Research process leading to the idea of ​​the step position detection device 1 according to the embodiment> When a vehicle is traveling, it is necessary to grasp the surrounding road surface condition and determine whether there are any risk factors. Disparity information contained in disparity images is used as a method for estimating the shape of the road surface. A method is known that applies this method to determine whether there is a step based on the unevenness of the road surface, as shown in Patent Document 1.

[0010] However, while step detection using parallax information can detect steps when they are located directly in front of and close to the stereo camera, it cannot handle road surfaces where the shape of the low position changes depending on the orientation of the camera relative to the low position, or road surfaces with slopes with large gradient angles.

[0011] Furthermore, there is a risk that the boundary cannot be detected if there are locally low points on the road surface or if the distance from the stereo camera to the step is long. This is because the road surface below the step cannot be observed due to occlusion, making it difficult to determine whether the height of the road surface actually changes suddenly from a boundary such as a step.

[0012] Furthermore, in a method that estimates the road surface using a two-dimensional map that assumes an arbitrary positional relationship between the step and the stereo camera, if there is a step on the side of the road surface, for example, the road surface information captured by the stereo camera will show two road surfaces, one above the step and one below the step. Therefore, a method that assumes a single road surface cannot detect the boundary.

[0013] Furthermore, on road surfaces where there is a diagonal step, the distance to the step varies depending on the actual lateral position, and the actual lateral position is not a uniform step, so the boundary cannot be detected.

[0014] Therefore, a step position detecting device 1 according to the following embodiment has been found that can solve such problems.

[0015] [Embodiment] <Configuration of step position detection device 1> 1 is an overall view of a work machine 2 that includes a step position detection device 1 according to one embodiment of the present invention. In the example shown in the figure, the work machine 2 is a forklift, but is not limited to this and may be another work machine 2 such as a wheel loader or a dump truck. In the following, the step position detection device 1 will be described as being attached to the work machine 2, but it may also be installed in a different location without being attached to the work machine 2.

[0016] The X direction of the work machine 2 is the forward direction. A point cloud information acquisition unit 21 is attached to the work machine 2. The point cloud information acquisition unit 21 may be an imaging means such as a stereo camera. The point cloud information acquisition unit 21 is attached to the rear side (-X direction side) of the work machine 2. The Y direction in the drawing is the height direction.

[0017] The point cloud information acquisition unit 21 acquires the situation around the work machine 2, including at least the road surface, as point cloud information. The surroundings of the work machine 2 may be the rear side of the work machine 2. Based on the point cloud information including height information acquired by the point cloud information acquisition unit 21, the step position detection device 1 can detect a first flat road on which the work machine 2 is located, and a second flat road that is lower than the first flat road.

[0018] The step position detection device 1 may be equipped with a notification means that notifies the driver of the work machine 2 when the step between the first flat road and the second flat road is equal to or greater than a predetermined value. The notification means is preferably a means that can warn the driver of the work machine 2 of the presence of a step and based on information about the position of the step. For this reason, examples of the notification means include, but are not limited to, a patrol lamp and an alarm.

[0019] 2 is a diagram showing the hardware configuration of a step position detection device 1 according to one embodiment of the present invention. As shown in the figure, the step position detection device 1 includes a point cloud information acquisition unit 21 and an image processing board 11.

[0020] The point cloud information acquisition unit 21 may have two imaging devices arranged in parallel. The imaging devices are, for example, stereo cameras. The point cloud information acquisition unit 21 includes a lens 12, an image sensor 13, and an image sensor controller 14. The point cloud information acquisition unit 21 is connected to the image processing board 11 via a data bus B1 and a serial bus B2.

[0021] The image sensor controller 14 controls the exposure of the image sensor 13, controls image readout, communicates with external circuits, and transmits image data. The luminance image data captured by the point cloud information acquisition unit 21 is transferred from the image sensor 13 to the RAM 18 of the image processing board 11 via the data bus B1. The serial bus B2 also changes the sensor exposure control values, changes image readout parameters, and transmits and receives various setting data from the CPU (Central Processing Unit) 15 and FPGA (Field-Programmable Gate Array) 16.

[0022] The image processing board 11 includes a CPU 15, an FPGA 16, a ROM (Read Only Memory) 17, a RAM (Random Access Memory) 18, a serial IF (Interface) 19, a data IF 20, a data bus B1, and a serial bus B2. The CPU 15 controls the overall operation of the image processing board 11 and performs image processing and image recognition processing.

[0023] The FPGA 16 performs real-time processing on the luminance image data stored in the RAM 18, such as gamma correction and distortion correction to parallelize the left and right images, and performs parallax calculations using block matching to generate a parallax image, which is written back to the RAM 18.

[0024] The CPU 15 loads programs for detecting road surface shapes and objects from the ROM 17. The CPU 15 executes various processes using the brightness image and parallax image stored in the RAM 18 as input, and outputs the detection data to the outside via the serial IF 19 or the data IF 20.

[0025] 3 is a functional block diagram of a level difference position detection device 1 according to one embodiment of the present invention. As shown in the figure, the level difference position detection device 1 includes a point cloud information acquisition unit 21, a road surface estimation unit 22, a level difference shape determination unit 23, an extended road surface estimation unit 24, and a level difference detection unit 25. The level difference shape determination unit 23 includes a road surface height calculation unit 26 and a level difference shape estimation unit 27. The functions of these units may be included in the CPU 15 or FPGA 16, or may be included in the image sensor controller 14.

[0026] The point cloud information acquisition unit 21 acquires the situation around the work machine 2, including at least the road surface, as point cloud information.

[0027] The road surface estimation unit 22 estimates, as a flat road, an area where the unevenness of the road surface does not exceed a predetermined height, based on point cloud information including height information acquired by the point cloud information acquisition unit 21. The road surface estimation unit 22 creates a two-dimensional map based on the point cloud information, and divides the two-dimensional map into a predetermined grid size to estimate the road surface. The road surface estimation unit 22 estimates the gradient of the second flat road relative to the first flat road.

[0028] The step shape determination unit 23 detects the boundary between the first flat road on which the work machine 2 is located, estimated by the road surface estimation unit 22, and a second flat road that is lower than the first flat road, and estimates the shape of the step from the position and angle of the boundary.

[0029] The road surface height calculation unit 26 calculates the road surface height by searching in two directions, left and right, at the actual lateral position of the point cloud information based on the result of estimation by the road surface estimation unit 22. The road surface height calculation unit 26 calculates the difference in road surface height between the first flat road and the second flat road, using the boundary as the height.

[0030] The step shape estimation unit 27 estimates the shape of the step line based on the calculated road surface height. The step shape estimation unit 27 estimates the shape of the boundary at the step based on the position of the boundary between the first flat road and the second flat road.

[0031] The extended road surface estimation unit 24 estimates the extended portion of the road surface that is hidden by a step on the first flat road on which the work machine 2 is located that has been estimated by the road surface estimation unit 22, and on a second flat road that is lower than the first flat road. The extended road surface estimation unit 24 extends the second flat road in the direction in which the road surface that is hidden by the step extends, and estimates the step between it and the first flat road.

[0032] The extended road surface estimation unit 24 estimates the extended portion of the road surface hidden by the step based on the attachment angle of the point cloud information acquisition unit 21 to the work machine 2. Note that, hereinafter, the attachment angle of the point cloud information acquisition unit 21 to the work machine 2 is also referred to as the camera attachment angle, and information including the camera attachment angle is also referred to as camera attachment angle information.

[0033] The step detection unit 25 performs a process of detecting steps based on the estimation result of the extended road surface estimation unit 24 of the extended portion of the road surface hidden by the step.

[0034] <Overall process flow> 4 is a flow diagram of processing performed by a step position detection device 1 according to one embodiment of the present invention. First, in the point cloud information acquisition processing of step S101, the point cloud information acquisition unit 21 acquires images of the surroundings of the work machine 2, including at least the road surface, captured by two imaging devices arranged side by side, and performs parallax calculation to generate a parallax image, which is point cloud information.

[0035] Next, in the road surface estimation process of step S102, the road surface estimation unit 22 creates a two-dimensional map based on the point cloud information and divides the two-dimensional map into a predetermined grid size. Then, the road surface estimation unit 22 estimates the road surface condition based on the height information within each grid. In the road surface estimation process, a process is performed to estimate the gradient of the second flat road relative to the first flat road.

[0036] In the road surface condition estimation process, the road surface condition assuming a flat road is calculated based on camera mounting angle information, which is the mounting angle of the imaging device relative to the work machine 2. For the calculated flat road surface, the road surface that appears on the parallax image is voted on a 2D map.

[0037] Next, in the step shape determination process of step S103, the step shape determination unit 23 extracts positions that are expected to be steps based on the road surface height of each grid, and can estimate the shape of the step line based on the extraction results.

[0038] The step shape estimation unit 27 estimates the shape of the step line based on the calculated road surface height. The step shape estimation unit 27 estimates the shape of the boundary at the step based on the position of the boundary between the first flat road and the second flat road and the angle state of the boundary.

[0039] Next, in the extended road surface estimation process of step S104, the extended road surface estimation unit 24 estimates the extended portion of the road surface that is hidden by a step on the first flat road on which the work machine 2 is located, estimated by the road surface estimation unit 22, and on a second flat road that is lower than the first flat road. The extended road surface estimation unit 24 extends the second flat road in the direction in which the road surface hidden by the step extends, and estimates the step between it and the first flat road. The extended road surface estimation unit 24 estimates the extended portion of the road surface that is hidden by the step based on the camera mounting angle information.

[0040] In the extended road surface estimation process, the road surface in the traveling direction is determined based on the estimated step shape. Therefore, by extending the road surface below the step in the extension direction, it is possible to estimate the road surface hidden by the step.

[0041] Finally, in the step detection process, the step detection unit 25 performs a process of detecting a step based on the estimation result of the extended portion of the road surface hidden by the step performed by the extended road surface estimation unit 24. More specifically, the step detection unit 25 estimates the road surface hidden by the step, recalculates the position of the step, and detects it as the step position.

[0042] The following describes in detail each process performed by the step position detection device 1.

[0043] <Point cloud information acquisition processing> The point cloud information acquisition unit 21 acquires point cloud information by performing parallax calculation. In the parallax calculation, the position in the depth direction as seen by the user can be adjusted by adjusting the parallax caused by the positional relationship between the left and right imaging devices. Then, a local matching process of the images is performed to detect the parallax.

[0044] The matching method may be, for example, block matching, which is a method in which a region of one image that has high similarity to a selected region of the other image is searched for, and the positional deviation from the region of high similarity is taken as parallax.

[0045] Image matching processes detect disparity on a pixel-by-pixel basis. Therefore, it is necessary to estimate disparity at the sub-pixel level, which is less than one pixel. Methods such as the equiangular linear method and the quadratic curve method are used for this estimation. These methods can be applied not only to imaging devices, but also to sensing devices that require the distance to the irradiation position, such as LiDAR and ToF sensors.

[0046] <Road surface estimation processing> Fig. 5 is a diagram for explaining the road surface estimation process using camera mounting angle information. Fig. 5 shows the relationship between the lens 12 and image sensor 13 in the imaging device of the point cloud information acquisition unit 21. In the figure, θ is the camera mounting angle, h is the camera mounting height, and f is the focal length of the lens 12. The image sensor 13 is placed at a position spaced apart from the lens 12 by the focal length f.

[0047] The road surface estimation unit 22 calculates the gradient of a flat road based on information such as the camera mounting height h and the camera mounting angle θ. If a flat road is defined as a road surface with a gradient of 0 degrees, the distance between the point cloud information acquisition unit 21 and the road surface changes when the camera mounting angle θ is not 0 degrees.

[0048] Therefore, based on the camera mounting angle information, the point cloud information acquisition unit 21 corrects the height by the camera mounting angle so that it is parallel to the flat road. With this correction, the image captured by the imaging device and focused on the image sensor 13 moves to area R in the figure. Therefore, the flat road is estimated without being affected by the camera mounting angle θ.

[0049] 6A and 6B are diagrams showing examples of a two-dimensional map created based on acquired point cloud information and a two-dimensional map divided into a predetermined grid size in a step position detection device 1 according to one embodiment of the present invention. In Fig. 6A, (a) is a parallax image which is the acquired point cloud information, (b) is a two-dimensional map created based on (a), and (c) is a two-dimensional map divided into a predetermined grid size.

[0050] The input image, which is point cloud information shown in Figure 6(a), includes a first flat road 31 on which the work machine 2 is located and a second flat road 32 that is lower than the first flat road 31. There is also a step 33 that serves as the boundary between the first flat road and the second flat road. Furthermore, walls 34 and 35 are included on the left and right of the first flat road 31.

[0051] As shown in Figures 6(b) and (c), the two-dimensional map is a diagram in which the vertical axis represents distance and the horizontal axis represents actual horizontal position. Also, as shown in Figure 6(c), the two-dimensional map is divided into a predetermined grid size. In Figure 6(c), g represents a grid.

[0052] The two-dimensional map is created by taking the actual distance on the vertical axis and voting the actual height corresponding to a position in real space. The reference for the height to be voted is the height of the flat road observed by the point cloud information acquisition unit 21. When the height of the flat road is used as the reference, the height of an object such as the road surface observed by the point cloud information acquisition unit 21 is voted on the two-dimensional map.

[0053] The height at the time of voting can be determined by voting the minimum height of each area observed by the point cloud information acquisition unit 21 on a two-dimensional map, thereby obtaining information including the road surface height around the point cloud information acquisition unit 21.

[0054] 7 is a diagram showing an example of the relationship between the actual distance and the corresponding actual height at a position in the real space on a flat road, in a step position detection device 1 according to one embodiment of the present invention. The vertical axis represents the actual height at the extracted position, and the horizontal axis represents the distance from the work machine 2. The circles in the diagram represent the actual height at each distance, and the dashed line represents the result of calculating the road surface gradient using linear approximation.

[0055] The road surface estimation unit 22 receives a two-dimensional map created based on point cloud information as input and can estimate the gradient of the road surface in each divided grid g. The continuity of the road surface height in each grid g is obtained based on data serving as sample points extracted from the height information voted for in each grid g in the two-dimensional map. Therefore, the road surface estimation unit 22 can estimate the road surface gradient based on the result of the linear approximation of the road surface height.

[0056] Here, if x is the distance from the work machine 2, the road surface gradient is expressed by the following equation (1). In equation (1), a and b are the slope and intercept, respectively, and are expressed by equations (2) and (3), respectively. n is the number of divided grids g.

[0057]

number

[0058] 8 is a flow diagram of the road surface estimation process in the step position detection device 1 according to one embodiment of the present invention. The road surface estimation unit 22 searches for data that will become sample points extracted from the height information voted for in each grid g in the two-dimensional map (step S201). The road surface estimation unit 22 calculates the gradient of the road surface in each grid g based on the height information at the sample points (step S202).

[0059] If the road surface gradient calculation process has not been completed for all grids g included in the two-dimensional map (No in step S203), the road surface estimation unit 22 continues the process, and if the road surface gradient calculation process has been completed for all grids g (Yes in step S203), the road surface estimation unit 22 completes the process.

[0060] <Step shape determination process> 9A and 9B are diagrams showing examples of step boundary surfaces for each grid g in step position detection according to one embodiment of the present invention. In Fig. 9A, an example where a step 33 is in the horizontal direction is shown, and Fig. 9B shows an example where a step 33 is in the traveling direction.

[0061] In the example of Figure 9(a), the area below the step 33 is a first flat road 31 on which the work machine 2 is located, and the area above the step 33 is a second flat road 32 that is lower than the first flat road 31. The above-step road surface grids 31g are the grids g that contact the boundary of the first flat road 31, and the below-step road surface grids 32g are the grids g that contact the boundary of the second flat road 32.

[0062] In the example of Figure 9(b), the left side of the step 33 is the first flat road 31 on which the work machine 2 exists, and the right side of the step 33 is the second flat road 32. The above-step road surface grids 31g are the grids g that contact the boundary of the first flat road 31, and the below-step road surface grids 32g are the grids g that contact the boundary of the second flat road.

[0063] The road surface height calculation unit 26 of the step shape determination unit 23 calculates the height of the road surface from the road surface gradient information at each grid g, and calculates the relative height between the grids g. If the relative height exceeds a predetermined height, the road surface height calculation unit 26 determines that there is a step at that position.

[0064] 9, the height of the above-step road surface grid 31g is higher than the below-step road surface grid 32g by a predetermined height or more, so the road surface height calculation unit 26 determines that there is a boundary that is a step between the above-step road surface grid 31g and the below-step road surface grid 32g.

[0065] FIG. 10 is a flowchart of a step shape determination process in the step position detection device 1 according to one embodiment of the present invention.

[0066] The step shape determination unit 23 performs two searches on the two-dimensional map divided into a predetermined grid size: one from left to right (hereinafter referred to as right search) and one from right to left (hereinafter referred to as left search), and records the number of steps determined in each search.

[0067] If the number of detected steps in the right search is greater than the number of detected steps in the left search (Yes in step S301), the step shape determination unit 23 estimates the angle value of the detected step line using the result of the right search (step S302). If the number of detected steps in the right search is equal to or less than the number of detected steps in the left search (No in step S301), the step shape determination unit 23 estimates the angle value of the detected step line using the result of the left search (step S303). Thereafter, the step shape determination unit 23 determines the step shape (step S304), and the step shape determination process flow ends.

[0068] The road surface height calculation unit 26 calculates the road surface height by searching in two directions, left and right, at the actual lateral position of the point cloud information. The step shape estimation unit 27 estimates the shape of the step line based on the calculated road surface height. The step shape estimation unit 27 can also estimate the distance to a step for each lateral position that cannot be expressed by the grid g by estimating the angle value of the step line.

[0069] Furthermore, since the step shape determination unit 23 calculates the road surface height by searching in two directions, left and right, it is possible to determine whether the step bottom is on the left or right side when there is a step top on the left side and a step bottom on the right side, or when there is a step bottom on the left side and a step top on the right side.

[0070] <Extended road surface estimation processing> FIG. 11 is a flowchart of the extended road surface estimation process in the step position detection device 1 according to one embodiment of the present invention.

[0071] The extended road surface estimation unit 24 extracts grids g of a predetermined line estimated by the step shape determination unit 23 on the first flat road 31 (step S401). Next, the extended road surface estimation unit 24 extracts grids g of a predetermined line determined by the step shape determination unit 23 on the second flat road 32 that is lower than the first flat road 31 (step S402).

[0072] If there are no grids g for which road surface estimation processing has not been performed between the grid g of the first flat road 31 and the grid g of the second flat road 32 (No in step S403), the process returns to step S401. If there are grids g for which road surface estimation has not been performed between the grid g of the first flat road 31 and the grid g of the second flat road 32 (Yes in step S403), the extended road surface estimation unit 24 performs road surface estimation processing for all grids g of the second flat road 32 (step S404). On the other hand, if there are no grids g for which road surface estimation has not been performed between the grid g of the first flat road 31 and the grid g of the second flat road 32 (No in step S403), the extended road surface estimation unit 24 performs the process of step S401. The extended road surface estimation unit 24 extracts grids g of a predetermined line estimated by the step shape determination unit 23 on the first flat road 31 (step S401).

[0073] After the processing of step S404, the extended road surface estimation unit 24 estimates the extended portion of the road surface hidden by the step 33 (step S405). If the search for the grids g of all lines is completed (step S406), the extended road surface estimation process is completed. On the other hand, if the search for all lines is not completed (No in step S406), the extended road surface estimation unit 24 performs the processing of step S401. The extended road surface estimation unit 24 extracts the grids g of the predetermined line estimated by the step shape determination unit 23 on the first flat road 31 (step S401).

[0074] In the extended road surface estimation process, the road surface below the step that cannot be observed due to the step 33 is determined in the road surface estimation result in the traveling direction. When the corresponding road surface is determined, the extended road surface estimation unit 24 determines the continuity of the gradient information of the second flat road 32 that has already been determined by the road surface estimation process.

[0075] As a method for this, the gradient information of the road surface determined to belong to the second flat road 32 is averaged. In the extended road surface estimation process, the averaged gradient information is used to estimate the road surface hidden by the step 33 for the grid g closest to the grid g located the farthest from the corresponding line on the second flat road 32.

[0076] <Step detection processing> 12 is a flowchart of the step detection process in the step position detection device 1 according to one embodiment of the present invention. The step detection unit 25 performs a process to detect the step 33 based on the estimation result of the extended road surface estimating unit 24 of the extended portion of the road surface hidden by the step 33.

[0077] The level difference detection unit 25 calculates the relative height between the grid points g on a predetermined line (step S501). If the calculated relative height exceeds the predetermined height (Yes in step S502), the level difference detection unit 25 records the position as a level difference 33 (step S503). On the other hand, if the calculated relative height exceeds the predetermined height (No in step S502), the level difference detection unit 25 performs the process of calculating the relative height between the grid points g on the predetermined line, which is the process of step S501 (step S501). Then, after recording in step S503, if the level difference detection unit 25 has completed the search on all lines (Yes in step S504), it ends the level difference detection process. On the other hand, if the search on all lines has not been completed (No in step S504), the level difference detection unit 25 performs the process of calculating the relative height between the grid points g on the predetermined line, which is the process of step S501 (step S501).

[0078] 13 is a conceptual diagram for explaining an example of detecting a step position in a step position detection device 1 according to one embodiment of the present invention. In Fig. 13, (a) shows the position of a step 33 on a two-dimensional map, and (b) shows the positional relationship between the work machine 2 and the step 33.

[0079] 13(a) and 13(b), the step position detection device 1 can notify the driver of the work machine 2 by outputting the distance from the work machine 2 to the step 33 and the actual lateral position. The notification means can warn the driver of the work machine 2 of the presence of the step 33 and information on the position of the step 33.

[0080] <Action and effect> The step position detection device 1 according to this embodiment acquires road surface information as three-dimensional information of lateral position, distance, and height, rather than two-dimensional information of distance and height. Therefore, the step 33 can be reliably detected regardless of the positional relationship between the imaging device (point cloud information acquisition unit 21) and the step 33. Furthermore, the extension of the road surface of the second flat road 32, which is located lower than the first flat road 31, that is hidden by the step 33 can be estimated.

[0081] Therefore, the step position detection device 1 according to this embodiment can accurately detect low positions not only when the shape of the low position changes or when there is a slope with a large gradient angle, but also when there is an extension of the road surface hidden by the step 33.

[0082] Although the embodiments have been described above, the present invention is not limited to the above-described embodiments, and various modifications and improvements are possible within the scope of the present invention.

[0083] Each function of the above-described embodiments can be realized by one or more processing circuits. Here, the term "processing circuit" in this specification includes a processor programmed to perform each function by software, such as a processor implemented by an electronic circuit, as well as devices such as an ASIC (Application Specific Integrated Circuit), a DSP (Digital Signal Processor), an FPGA (Field Programmable Gate Array), and conventional circuit modules designed to perform each of the above-described functions.

[0084] For example, aspects of the present invention are as follows. <1> a point cloud information acquisition unit that acquires a situation around the work machine including at least the road surface as point cloud information; a road surface estimation unit that estimates, as a flat road, an area where the unevenness of the road surface does not exceed a predetermined height, based on point cloud information including height information acquired by the point cloud information acquisition unit; an extended road surface estimation unit that estimates an extended portion of the road surface that is hidden by a step on a first flat road on which the work machine is located that has been estimated by the road surface estimation unit and a second flat road that is lower than the first flat road; and A step position detection device comprising: <2> The road surface estimation unit creating a two-dimensional map based on the point cloud information, and dividing the two-dimensional map into a predetermined grid size to estimate the road surface; The aforementioned <1> The step position detection device according to claim 1. <3> The road surface estimation unit Estimating a gradient of the second flat road relative to the first flat road; The aforementioned <1> or the above <2> The step position detection device according to claim 1. <4> The extended road surface estimation unit extending a second flat road in a direction in which the road surface hidden by the step extends, and estimating the step between the second flat road and the first flat road; The aforementioned <1> From the above <3> 10. The step position detection device according to claim 9, wherein: <5> The extended road surface estimation unit an extension of the road surface hidden by the step is estimated based on an attachment angle of the point cloud information acquisition unit to the work machine; The aforementioned <1> From the above <4> 10. The step position detection device according to claim 9, wherein: <6> The point cloud information acquisition unit includes a stereo camera. The aforementioned <1> From the above <5> 10. The step position detection device according to claim 9, wherein: <7> The vehicle further includes a notification means for notifying the driver when the step between the first flat road and the second flat road is equal to or greater than a predetermined value. The aforementioned <1> From the above <5> 10. The step position detection device according to claim 9, wherein: <8> a point cloud information acquisition unit that acquires a situation around the work machine including at least the road surface as point cloud information; a road surface estimation unit that estimates, as a flat road, an area where the unevenness of the road surface does not exceed a predetermined height, based on point cloud information including height information acquired by the point cloud information acquisition unit; a step shape determination unit that detects a boundary between a first flat road on which the work machine is located, estimated by the road surface estimation unit, and a second flat road that is lower than the first flat road, and estimates the shape of a step from the position and angle of the boundary; A step position detection device comprising: <9> The step shape determination unit a road surface height calculation unit that calculates a road surface height by searching in two directions, left and right, at the actual lateral position of the point cloud information based on the result of estimation by the road surface estimation unit; a step shape estimation unit that estimates the shape of a step line based on the calculated road surface height; Including, The aforementioned <8> The step position detection device according to claim 1. <10> The road surface height calculation unit Calculating the difference in road surface height between the first flat road and the second flat road using the boundary as a height. The aforementioned <9> The step position detection device according to claim 1. <11> The step shape estimation unit estimating a shape of the boundary at the step based on a position of the boundary between the first flat road and the second flat road; The aforementioned <9> or the above <10> The step position detection device according to claim 1. [Explanation of symbols]

[0085] 1. Step position detection device 2. Work machinery 21 Point cloud information acquisition section 22 Road surface estimation section 23 Step shape determination unit 24 Extended road surface estimation section 25 Step detection unit 26 Road surface height calculation unit 27 Step shape estimation section 31 First Flat Road 32 Second Flat Road 33 Steps [Prior art documents] [Patent documents]

[0086] [Patent Document 1] Japanese Patent Application Laid-Open No. 2020-56717

Claims

1. a point cloud information acquisition unit that acquires a situation around the work machine including at least the road surface as point cloud information; a road surface estimation unit that estimates, as a flat road, an area where the unevenness of the road surface does not exceed a predetermined height, based on point cloud information including height information acquired by the point cloud information acquisition unit; an extended road surface estimation unit that estimates an extended portion of the road surface that is hidden by a step on a first flat road on which the work machine is located that has been estimated by the road surface estimation unit and a second flat road that is lower than the first flat road; and A step position detection device comprising:

2. The road surface estimation unit creating a two-dimensional map based on the point cloud information, and dividing the two-dimensional map into a predetermined grid size to estimate the road surface; The step position detection device according to claim 1 .

3. The road surface estimation unit estimating a gradient of the second flat road relative to the first flat road; The step position detection device according to claim 1 .

4. The extended road surface estimation unit extending a second flat road in a direction in which the road surface hidden by the step extends, and estimating a step between the second flat road and the first flat road; The step position detection device according to claim 1 .

5. The extended road surface estimation unit an extension of the road surface hidden by the step is estimated based on an attachment angle of the point cloud information acquisition unit to the work machine; The step position detection device according to claim 1 .

6. The point cloud information acquisition unit includes a stereo camera. The step position detection device according to any one of claims 1 to 5.

7. The vehicle further includes a notification means for notifying the driver when the difference in level between the first flat road and the second flat road is equal to or greater than a predetermined value. The step position detection device according to any one of claims 1 to 5.

8. a point cloud information acquisition unit that acquires a situation around the work machine including at least the road surface as point cloud information; a road surface estimation unit that estimates, as a flat road, an area where the unevenness of the road surface does not exceed a predetermined height, based on point cloud information including height information acquired by the point cloud information acquisition unit; a step shape determination unit that detects a boundary between a first flat road on which the work machine is located, estimated by the road surface estimation unit, and a second flat road that is lower than the first flat road, and estimates the shape of a step from the position and angle of the boundary; A step position detection device comprising:

9. The step shape determination unit a road surface height calculation unit that calculates a road surface height by searching in two directions, left and right, at the actual lateral position of the point cloud information based on the result of estimation by the road surface estimation unit; a step shape estimation unit that estimates the shape of a step line based on the calculated road surface height; Including, The step position detection device according to claim 8 .

10. The road surface height calculation unit Calculating a difference in road surface height between the first flat road and the second flat road using the boundary as a height. The step position detection device according to claim 9.

11. The step shape estimation unit estimating a shape of the boundary at the step based on a position of the boundary between the first flat road and the second flat road; The step position detection device according to claim 9.

Images