JP2013254474A - Obstacle detector - Google Patents

Abstract

PROBLEM TO BE SOLVED: To detect an obstacle on a road surface on which a movable body travels even if a depth sensor swings to cause a change in an angle of depression during travel of the movable body.SOLUTION: An obstacle detector 1 includes: a depth sensor 2; a depression angle estimation unit 3 that estimates an angle of depression α of the depth sensor 2 on the basis of a distance image acquired by the depth sensor 2; and a detection unit 4 that detects an obstacle 8 present on a road surface 6 on the basis of the angle of depression α, the height h of the depth sensor 2 and a distance r of the distance image.

Description

  The present invention relates to an obstacle detection apparatus including a depth sensor that is attached to a moving body including a vehicle and a pedestrian and acquires a distance image of a road surface.

  In this specification, “moving body” includes “vehicle” and “pedestrian”.

  The following configuration is known as a conventional obstacle detection device. That is, there is known a traveling control method for an electric wheelchair in which an obstacle detection sensor using a laser detects an obstacle and stops traveling (Patent Document 1). Then, an image output from a sensor device that observes a road surface or a floor surface in the vicinity of the vehicle is analyzed and its characteristics are output, and the characteristics are compared with the history of the road surface on which the vehicle has traveled in the past. A configuration for estimating road safety is disclosed (Patent Document 2).

  In addition, the direction in which the obstacle exists is calculated based on the temporal change in the distance to the obstacle obtained from the distance sensor using a laser attached to the vehicle and the speed obtained from the speed detecting means. A configuration for performing an avoidance operation according to this direction is taught (Patent Document 3). In addition, while the vehicle is running, a vehicle front scan is performed by a two-dimensional laser radar to recognize and store dangerous parts such as steps and grooves, to detect the rotational speed of the drive wheels, to calculate and store the amount of movement of the vehicle. None, when the collision prediction by the collision prediction means is performed based on the memory danger location and the momentary vehicle movement amount information and the vehicle movement direction, a collision avoidance correction command is issued, and the setting of the operator's handle angle, A configuration is known in which correction is performed so that the vehicle does not collide with the accelerator operation (Patent Document 4).

  And the structure of the self-position identification method and apparatus (SLAM (Simultaneous Localization And Mapping)) which estimates a self position using a distance sensor and produces a map is also disclosed (patent document 5).

  The only way to safely drive a senior car (electric car for the elderly) in dark hours is to use a bicycle or a light to watch the person in front, but in the first place, people who use a senior car have low vision and concentration. In many cases, it was falling, and I had to run in a very dangerous state. The present inventors disclosed a system for detecting unevenness on a road surface using one image camera as a method for automatically detecting an obstacle on the road surface using a machine (Non-Patent Document 1).

JP 2011-177205 A (released on September 15, 2011) JP 2009-277063 A (released on November 26, 2009) JP 2005-335533 A (released on December 08, 2005) JP 2005-128722 A (published May 19, 2005) WO 2007/069726 (released on June 21, 2007)

Detection of Flat Ground Area by Single Camera on Mobile Vehicles, 2011 International Conference on Advanced Mechatronic Systems, Zhongyuan University of Technology, Zhengzhou, China, August 11-13, 2011 pp. 403-408, M. Tanaka and S. Onishi

  However, since the configurations disclosed in Patent Documents 1 to 4 are a method of acquiring distance image data for one screen by scanning a laser beam, distance image data for one screen is acquired by one scan. For example, a long time of about several hundred milliseconds is required. Accordingly, there is a problem that it is difficult to obtain and process a road surface distance image by attaching to a moving body moving outdoors.

  The configuration of the SLAM disclosed in Patent Document 5 has a problem that the calculation load is large and the configuration is complicated.

  In the configuration disclosed in Non-Patent Document 1, it is essential that the moving body on which the camera is mounted is traveling on the road surface, and the obstacle detection capability is low. In this configuration, feature points are extracted with the help of computer vision techniques (SURF, SIFT, etc.), and the points between the points above and below the road surface are discriminated by matching them between frames. There are many problems such as feature points not being able to be taken and correspondence not necessarily succeeding, and practical application has been difficult.

  A configuration is also possible in which distance image data for one screen at the same moment is acquired by a sensor using infrared rays or the like. However, since a moving body that is traveling vibrates rattlingly, the depression angle of the sensor cannot be specified, and an obstacle on the road surface There is a problem that it is difficult to detect.

  There is also a desire to use the obstacle detection device by attaching it to the body of a pedestrian with visual impairment.

  An object of the present invention is to provide an obstacle detection device that can detect an obstacle on a road surface on which a mobile body travels even if the depth sensor fluctuates and the depression angle fluctuates during travel of the mobile body.

  Further, in the present invention, an obstacle detection device capable of detecting an obstacle on the road surface on which the mobile body travels even if the depth sensor fluctuates and the depression angle, roll angle, and height fluctuate during travel of the mobile body including a pedestrian. Is to provide.

  In order to solve the above problems, an obstacle detection device according to the present invention is attached to a vehicle based on a depth sensor that is attached to the vehicle and acquires a distance image of a road surface, and a distance image acquired by the depth sensor. An angle estimating means for estimating a depression angle of the depth sensor, a depression angle of the depth sensor estimated by the angle estimation means, a height of the depth sensor from the road surface, and distance data of the distance image. And detecting means for detecting an existing obstacle.

  In this specification, the “depth sensor” means a distance sensor that can acquire surface information. Examples of such depth sensors that are commercially available include Kinect for Windows (registered trademark) and Xtion Pro (registered trademark). However, it is not limited to these.

  With this feature, the depression angle of the depth sensor attached to the moving body is estimated based on the distance image acquired by the depth sensor, and based on the estimated depression angle, the height of the depth sensor from the road surface, and the distance image distance data. And detecting obstacles present on the road surface. For this reason, since the depression angle of the depth sensor at the moment when the depth sensor acquires the distance image can be estimated, even if the depression sensor fluctuates and the depression angle fluctuates during traveling of the moving body, an obstacle on the road surface on which the moving body travels is detected. be able to.

  In the obstacle detection device according to the present invention, it is preferable that the angle estimation means estimates the depression angle by minimizing an error based on a least square method that removes the influence of a point on the non-road surface.

  According to the above configuration, the depression angle of the depth sensor attached to the moving body can be estimated with a simple configuration.

  In the obstacle detection device according to the present invention, the vehicle has a handle, and the angle estimating means rolls the depth sensor according to the depression angle and the rotation of the handle according to the rotation of the handle. It is preferable to estimate the angle.

  According to the above configuration, an obstacle on the road surface can be detected even when the handle is rotated and the moving body bends.

  In the obstacle detection apparatus according to the present invention, the depth sensor is preferably a non-scanning infrared laser sensor.

  According to the above configuration, the depth sensor can be configured at low cost.

  The obstacle detection device according to the present invention includes a depth sensor that is attached to a pedestrian and acquires a distance image of a road surface, and a depression angle and a roll of the depth sensor that is attached to the pedestrian based on the distance image acquired by the depth sensor. An obstacle existing on the road surface based on state estimation means for estimating the angle and height, the depression angle, roll angle and height of the depth sensor estimated by the state estimation means, and the distance data of the distance image And detecting means for detecting.

  With this feature, the depression angle, roll angle, and height of the depth sensor attached to the moving body are estimated based on the distance image acquired by the depth sensor, and the estimated depression angle, roll angle, height, Based on the distance data, an obstacle present on the road surface is detected. For this reason, since the depression angle, roll angle, and height of the depth sensor at the moment when the depth sensor acquires the distance image can be estimated, the depth sensor is shaken during traveling of a moving body including a pedestrian, and the depression angle, roll angle, and height are Even if fluctuates, it is possible to detect an obstacle on the road surface on which the mobile body travels.

  In the obstacle detection apparatus according to the present invention, the state estimation means estimates the depression angle, the roll angle, and the height by minimizing an error based on a least square method that eliminates the influence of a point on the non-road surface. It is preferable to do.

  According to the above configuration, it is possible to estimate the depression angle, the roll angle, and the height of the time-varying depth sensor attached to the moving body with a simple configuration.

  In the obstacle detection device according to the present invention, the state estimation unit may affect the influence of a point on the non-road surface on the state space model based on the relationship between the distance data, the depression angle, the roll angle, and the height. It is also possible to estimate the depression angle, the roll angle, and the height while allowing variation based on the removed nonlinear Kalman filter.

  According to the above configuration, even in an environment where the depression angle, roll angle, and height of the depth sensor attached to the moving body vary, the variation angle can be incorporated in the model to estimate the depression angle, roll angle, and height.

  Note that the algorithm used in the estimation of the depression angle, the roll angle, and the height is not limited to the nonlinear Kalman filter, and the depression angle, the roll angle, and the height may be estimated based on other filtering algorithms. Good.

  In addition, the set of parameters for determining the posture of the depth sensor is not limited to the set of depression angle, roll angle, and height as long as it determines the state of the sensor.

  An obstacle detection device according to the present invention includes an angle estimation unit that estimates a depression angle of a depth sensor attached to the vehicle based on a distance image acquired by the depth sensor, and a depth sensor estimated by the angle estimation unit. Since a depression angle, a height of the depth sensor from the road surface, and detection means for detecting an obstacle present on the road surface based on the distance data of the distance image are provided, the moment when the depth sensor acquires the distance image The depression angle of the depth sensor can be estimated, and an obstacle on the road surface on which the mobile body travels can be detected even if the depression sensor fluctuates and the depression angle fluctuates during vehicle travel.

  An obstacle detection device according to the present invention includes a depth sensor that is attached to a pedestrian and acquires a distance image of a road surface, and a depth sensor that is attached to the pedestrian based on the distance image acquired by the depth sensor. Present on the road surface based on state estimation means for estimating depression angle, roll angle and height, depression angle, roll angle and height of the depth sensor estimated by the state estimation means, and distance data of the distance image And detecting means for detecting obstacles to be able to estimate the depression angle, roll angle, and height of the depth sensor at the moment when the depth sensor acquires the distance image. Even if the roll angle and the height fluctuate, an obstacle on the road surface on which the pedestrian walks can be detected.

It is a figure which shows typically the structure of the obstruction detection apparatus which concerns on Embodiment 1. FIG. It is a figure which shows the external appearance of the said obstacle detection apparatus. It is a figure which shows the coordinate system for demonstrating the algorithm of operation | movement of the said obstacle detection apparatus. It is a schematic diagram for demonstrating the target screen from which the depth sensor provided in the said obstacle detection apparatus acquires data. (A) is a figure for demonstrating the measurement data of a depth sensor when there is no obstacle and a groove | channel on a road surface, (b) is for demonstrating the measurement data of a depth sensor when an obstacle exists on a road surface. It is a figure, (c) is a figure for demonstrating the measurement data of a depth sensor in case there exists a groove | channel on a road surface. It is the figure which looked at the obstacle which exists in the road surface ahead from the driver's seat of the said obstacle detection apparatus. It is a graph which shows the result of having estimated the height of the road surface along line L1 shown in FIG. It is a graph which shows the result of having estimated the height of the road surface along line L2 shown in FIG. It is a figure which shows typically the structure of the obstruction detection apparatus which concerns on Embodiment 2. FIG. FIG. 10 is a diagram for explaining the operation of the obstacle detection device according to the second embodiment. It is a figure which shows typically the structure of the obstruction detection apparatus which concerns on Embodiment 3. FIG. It is a figure for demonstrating the usage pattern of the obstruction detection apparatus shown in FIG. It is a figure for demonstrating the other usage pattern of the obstruction detection apparatus shown in FIG. It is a figure which shows a coordinate system when the depth sensor in a rectangular coordinate and a polar coordinate rotates with a predetermined roll angle. It is a figure which shows the image | video in the obstacle detection in the obstacle detection apparatus which concerns on Embodiment 4. FIG. (A) is a graph which shows transition of the estimated value of the depression angle of the depth sensor provided in the said obstacle detection apparatus, (b) is a graph which shows transition of the estimated value of the roll angle of the said depth sensor, (c) These are the graphs which show the transition of the estimated value of the height of the depth sensor. It is a figure which shows the scooter to which the said obstacle detection apparatus is applied.

  Hereinafter, the obstacle detection device according to the embodiment of the present invention will be described in detail.

(Overview)
When trying to run a senior car (electric car for the elderly) at twilight or at night, it is difficult to visually recognize a particularly close place with the front light alone, which is extremely dangerous. Therefore, in the following embodiment, an obstacle detection device that can detect the danger of the road surface in the traveling direction and teach the driver in a dim or dark situation at twilight or at night is a Kinect (Kinect ( Registered trademark)) Realized using sensors.

(Embodiment 1)
(Configuration of the obstacle detection device 1)
FIG. 1 is a diagram schematically illustrating a configuration of an obstacle detection apparatus 1 according to the first embodiment. FIG. 2 is a diagram illustrating an appearance of a moving body (vehicle) 5 on which the obstacle detection device 1 is mounted.

  The moving body 5 is, for example, a senior car, and travels on the road surface 6 with the obstacle detection device 1 mounted thereon. The obstacle detection device 1 includes a depth sensor 2. The depth sensor 2 is attached to a bag 10 (FIG. 2) provided in front of the handle 7 (FIG. 2) of the moving body 5 and acquires a distance image of the road surface 6.

  The obstacle detection device 1 is provided with a depression angle estimation unit (angle estimation means) 3. The depression angle estimation unit 3 estimates the depression angle α based on the horizontal line 9 of the depth sensor 2 attached to the moving body 5 based on the distance image of the road surface 6 acquired by the depth sensor 2. The depression angle estimation unit 3 estimates the depression angle α by minimizing the error based on the least square method that eliminates the influence of points on the non-road surface.

  The obstacle detection device 1 includes a detection unit 4. Based on the depression angle α of the depth sensor 2 estimated by the depression angle estimation unit 3, the height h of the depth sensor 2 from the road surface 6, and the distance r of the distance image acquired by the depth sensor 2, the detection unit 4. Is detected.

  The obstacle detection device 1 is provided with a display 11 and a speaker 12. The display 11 displays information representing the obstacle 8 detected by the detection unit 4. The speaker 12 outputs a sound or sound for notifying the obstacle 8 detected by the detection unit 4.

  The moving body 5 includes a battery 13. The battery 13 supplies power for the moving body 5 to travel, and supplies power for operating the depth sensor 2, the depression angle estimation unit 3, and the detection unit 4.

(Characteristics of depth sensor 2)
The depth sensor 2 is inexpensive and has the ability to acquire not only images but also distance images. Moreover, it has a high resolution of 640 × 480, which is the same resolution as the image.

  The interval of data acquisition by the depth sensor 2 is about 30 fps, which is equivalent to a general image camera. The distance that can be acquired is about 5-6m. However, the depth sensor 2 is weak in daylight, and data cannot be acquired outdoors when exposed to direct sunlight.

  The above-described characteristics of the depth sensor 2 are suitable for the purpose of the present embodiment for detecting road surface obstacles by a senior car. In other words, the distance that can acquire a distance image of 5-6m is sufficient for pedestrians and senior cars that can only run at speeds of 6km / h or less, and the sensor characteristics of the depth sensor 2 that are more convenient in the dark are darker. It suits the purpose of detecting road obstacles that can be used occasionally. The depression angle estimation unit 3 and the detection unit 4 can be configured by a notebook PC. Since this notebook PC can be configured to be removable from the mobile unit 5, it is sufficient to attach an already owned PC to the mobile unit 5 and use it only when necessary. The notebook PC, PDA (Personal Digital Assistant) ), And there is no need to purchase a new portable data processing device such as a smart phone. Therefore, it can be said that the cost of the obstacle detection apparatus 1 provided with the depth sensor 2 is sufficiently low as an option for a mobile body (senior car). Although the depth sensor 2 is widely recognized as being used indoors, the present inventor applied the depth sensor 2 to the obstacle detection device 1 mounted on a senior car traveling outdoors, paying attention to the above-described characteristics of the depth sensor 2. The invention which uses such a depth sensor 2 does not see an example.

(How to use depth sensor 2)
As shown in FIG. 2, the depth sensor 2 is fixedly attached to the upper side of the heel 10 provided on the front side of the handle 7 of the movable body 5 (senior car) at a depression angle of about 20-40 degrees. Thereby, the state ahead of the mobile body 5 from several tens cm to several m can be acquired as a distance image. The depth sensor 2 requires a 12V DC power supply. This can be easily prepared by sharing the power source of the mobile unit 5. Moreover, in order to process the distance image data acquired by the depth sensor 2, a notebook personal computer is necessary, but this can also be stored by folding it and putting it in the bag 10. A monitor display 11 (FIG. 1) of about 7-9 inches is attached to the handle 7 in order to convey information on the detected obstacle (object) to a person. A sound can be emitted from the speaker 12 in response to the detection of the obstacle 8.

  When the depth sensor 2 is attached to the eaves 10, it shakes considerably when the moving body 5 travels. However, since the obstacle 8 can be detected from one distance image, there is basically no problem even if it shakes.

  Although the depth sensor is now specified, the present invention is not limited to this. In general, other sensors may be used as long as they can capture distance data on a plane (not information on a line acquired by a one-dimensional laser scanner that scans a one-dimensional line).

  If distance image data for one screen can be acquired in a short time by one scan, it is attached to a moving body such as a vehicle or a person moving outdoors to acquire and process a distance image of the road surface. Therefore, a laser scanning type distance sensor may be used.

  Furthermore, although the example which mounts the obstruction detection apparatus 1 in the mobile body 5 is given, this invention is not limited to this. It may be configured to be small and light so as to be worn on the waist of a pedestrian walking out at night. Thereby, there exists an effect that a pedestrian can detect the presence or absence of the obstruction on the road surface at night.

  How large an object (obstacle) is captured depends on the sensor error. The object captured by the sensor is assumed to be an object or a step that becomes a major obstacle to the traveling of the moving body 5.

  A feature of the algorithm according to the present embodiment is that the depression angle α is estimated for each screen acquired by the depth sensor 2. Therefore, this algorithm is strong against the shaking of the depth sensor 2 according to the traveling of the moving body 5. Further, the depth sensor 2 is tilted when the handle 7 is turned. However, as will be described later in Embodiment 2, the roll angle can also be estimated. When the handle 7 is turned, the obstacle 8 in the traveling direction can be estimated. Can be found.

  As shown in FIG. 2, the depth sensor 2 is used by being attached to the cage 10. Alternatively, it can be attached to the handle 7. Due to the nature of the depth sensor 2, if the angle between the viewing direction of the depth sensor 2 and the road surface 6 is set to an acute angle, the road surface 6 cannot be detected well. Therefore, in addition to the configuration shown in FIG. 2, a configuration in which the depth sensor 2 is mounted at an ideal angle by attaching a mounting tool dedicated to the sensor to the front of the moving body 5 is also possible. If safety is proven, it will look smarter when embedded in the body as part of a senior car structure. In the example of the first embodiment, the processing PCs that make up the depression angle estimation unit 3 and the detection unit 4 are folded into the basket 10. In addition, when mounting | wearing a pedestrian, you may attach to a human body by the method which can be used for a waist, a face, a head, etc. using a suitable attachment tool.

(Depth Sensor 2 Distance Image Data Processing Method)
By attaching a sensor (here, the depth sensor 2 is assumed) that can measure the distance data to the road surface 6 in a plane, to the eaves 10 of the moving body 5 (senior car), etc. Unevenness that obstructs the traveling of the moving body 5 such as an obstacle 8, a hole, or a groove is detected by one sensor and the processing PC (the depression angle estimation unit 3 and the detection unit 4).

  The depth sensor 2 is designed for indoor use, and there is a possibility that data cannot be acquired sufficiently due to the characteristics of the sensor when exposed to sunlight outdoors. However, if there is a shadow, the data can be acquired. Otherwise, it can be used in Hyuga. From evening to night, the sensing ability is as high as indoors, even outdoors. In particular, in order to enhance the sensing ability, the distance data to the road surface can be obtained without any problems by attaching a certain degree of depression angle (assuming about 30 degrees) to the relatively high part (the upper side of the fence 10). Can be measured.

  The distance to the road surface at the position on each grid point (pixel) obtained from the depth sensor 2 can be easily obtained from the relationship between polar coordinates and orthogonal coordinates if the depression angle α of the depth sensor 2 is known. . Therefore, by comparing the theoretical value with the actually measured value, it can be determined whether the point is on the road surface 6, a point higher than the road surface 6, or a low point.

  However, since the moving body 5 (senior car) swings considerably during traveling, it is inevitable that the depth sensor 2 itself attached to the saddle 10 also swings, and it is difficult to make the depression angle α known.

  For this reason, in the present embodiment, by utilizing the fact that many points are on the road surface 6, by adopting a method that removes the influence of points on the non-road surface, such as a robust least square method or RANSAC, the error is eliminated. Minimize and estimate the depression angle α. If the depression angle α can be accurately estimated, the unevenness of the road surface 6 can be detected by the method described below. A model can also be created when the handle is turned and there is a roll angle.

(Algorithm for moving forward)
FIG. 3 is a diagram showing a coordinate system for explaining an algorithm of operation of the obstacle detection apparatus 1. FIG. 4 is a schematic diagram for explaining the target screen S from which the depth sensor 2 provided in the obstacle detection apparatus 1 acquires data. The depth sensor 2 obtains a distance at a grid point on the target screen S on the road surface 6 with vertical nV = 480 dots (viewing angle V = 43 degrees) and horizontal nH = 640 dots (viewing angle H = 57 degrees). It is a sensor that can. If the lower left corner of the screen is the origin ORG, the vertical direction is i = 0,..., NV-1, and the horizontal direction is j = 0,. Let the three-dimensional space be (x, y, z). When the space is expressed by polar coordinates (r, θ, φ) (see FIG. 3), the following (formula 1)

It becomes.

  Now, the vertical pixels of the depth sensor 2 equally divide the vertical viewing angle V into nV−1, and similarly, the horizontal pixels equal the horizontal viewing angle H to nH−1. Is divided into In addition, the height h of the depth sensor 2 is substantially constant, and the orthogonal coordinates have a road surface z = 0.

  The front in the traveling direction is φ = 0. The angle directly above is θ = 0, and the front is θ = 90 degrees. At this time, the following (Formula 2)

(See FIG. 3). The depression angle is α-90 degrees.

  Polar coordinate angle of a point corresponding to each pixel (i, j) (i = 0,..., NV−1, j = 0,..., NH−1) on the target screen S viewed from the light emitting point L of the depth sensor 2. θi and φi are expressed by the following (formula 3) and (formula 4).

  The unit of these angles is degrees (°).

The depth sensor 2 has a function of observing the distance r _ij between the point corresponding to each pixel (i, j) and the light emitting point L. Therefore, using the fact that the road surface directly below the position of the depth sensor 2 is the origin and the distance from the sensor position to the road surface 6 is always a constant value h, corresponding to z and r cos θ + h in the third column of (Equation 2). The following (Formula 5)

Should hold.

  From this relational expression (Formula 5), the depression angle α is estimated by the following method.

  FIG. 5A is a diagram for explaining measurement data of the depth sensor 2 when there are no obstacles and grooves on the road surface 6, and FIG. 5B is a diagram of the depth sensor 2 when the obstacle 8 is on the road surface 6. It is a figure for demonstrating measurement data, (c) is a figure for demonstrating the measurement data of the depth sensor 2 in case the groove | channel 14 exists on the road surface 6. FIG.

In a single distance image, the depression angle α is considered to be constant (in the case of a scanning laser sensor, since it takes time to acquire one distance data by shaking the head, the depression angle α is It ’s not constant.) Further, at a point P1 reflected from the road surface 6 shown in FIG. 5A, z _ij = 0. However, when an object (obstacle 8) is on the road surface 6 as shown in FIG. 5B, or a point lower than the road surface 6 such as a point in the groove 14 as shown in FIG. 5C. Z _ij is not equal to 0 when reflected from. When the depression angle α is unknown, it is not known at which position (i, j) z _ij is not equal to zero. However, since the road surface 6 is assumed to be a road on which the moving body 5 travels, many positions should be reflected from the top of the road surface 6 as shown in FIG. The depression angle α can be determined.

  1. Assuming that all the points are on the road surface 6, the depression angle α that minimizes the square error is determined numerically. That is,

Ask for.
2. Using the obtained depression angle α,

Calculate

In a set S of positions (i, j) excluding positions (i, j) such that

Ask for. This is repeated until convergence.

  If the number of points on the plane (road surface 6) is so small that the above method does not work, a robust algorithm such as RANSAC may be applied.

  The above 1. Since the optimization by the depression angle α is not accompanied by a constraint condition, a simple one-dimensional search method such as the golden section method can be applied. The depression angle α may be obtained in a section such as [110 degrees, 130 degrees].

(Estimation of height)
Using the obtained estimated value of the depression angle α, an estimated value of z _{ij in} the height direction is obtained from (Equation 2). In addition, x _ij , y _ij (obstruction 8, plane position of the groove 14 on the road surface 6) can be estimated at the same time. Therefore, the height (obstacle 8) or the exact planar position of the groove 14 can be known.

(Road height estimation experiment results)
FIG. 6 is a view of the obstacle 8 existing on the road surface 6 ahead from the driver's seat of the obstacle detection apparatus 1. FIG. 7 is a graph showing a result of estimating a plurality of heights of the road surface 6 in parallel along the line L1 shown in FIG. The horizontal axis indicates the pixel number along the vertical direction of the distance image acquired by the depth sensor 2, the right side of the horizontal axis indicates the near side in FIG. 4, that is, the lower side of the distance image, and the left side is the figure. 4 represents the far side, that is, the upper side of the distance image. The vertical axis indicates the height from the road surface 6.

  In the line C1 in FIG. 7, the left end portion having a height of 680 mm is a portion where data is missing and can be excluded. When these points are removed, the height from the road surface 6 corresponding to the pixel numbers 1 to 90 and the pixel numbers 160 to 480 is substantially zero. The height from the road surface 6 increases from the pixel number of about 160 to the pixel number of about 100, and the height corresponding to the pixel numbers of about 100 to about 90 is about 200 mm.

  The increase in the height of the pixel numbers of about 160 to about 100 is considered because the infrared rays from the depth sensor 2 are reflected by the side surface 15 (FIG. 5B) on the depth sensor 2 side of the obstacle 8. The height 200 of the pixel numbers of about 100 to about 90 is considered to be because the infrared rays from the depth sensor 2 are reflected by the upper surface 16 (FIG. 5B) of the obstacle 8 having a height of 200 mm. Thus, the obstacle 8 can be detected by detecting the height from the road surface 6 corresponding to the pixel number.

  FIG. 8 is a graph showing a result of estimating the height of the road surface 6 along a plurality of lines in the vicinity of the obstacle 8 along the line L2 shown in FIG. The horizontal axis indicates the pixel number along the horizontal direction of the distance image acquired by the depth sensor 2, the right side of the horizontal axis represents the right side in FIG. 4, that is, the right side of the distance image, and the left side in FIG. The left side, that is, the left side of the distance image is shown. The vertical axis indicates the height from the road surface 6.

  As indicated by line C2 in FIG. 8, the portion displayed as height 680 is missing data. Therefore, if these are removed, the height from the road surface 6 corresponding to pixel numbers 1 to 350 and pixel numbers 420 to 640 is removed. The depression angle is not optimized and a little drift remains, but the depression angle at which the depression angle becomes horizontal can be estimated as described above. The height from the road surface 6 corresponding to the pixel number from about 350 to about 420 is about 200 mm, and can be clearly distinguished from the points on the road surface 6.

  The height 200 of the pixel numbers of about 350 to about 420 is considered to be because the infrared rays from the depth sensor 2 were reflected by the upper surface 16 (FIG. 5B) of the obstacle 8 having a height of 200 mm. Thus, the obstacle 8 can be detected by detecting the height from the road surface 6 corresponding to the pixel number.

(Effect of embodiment)
According to the method of the present embodiment, since a sensor capable of acquiring distance data is used, the distance can be measured more stably, and the uneven portion of the road surface 6 while the moving body 5 is stationary. Can be detected.

  Moreover, the power source of the depth sensor 2 can be acquired from the senior car body so that it can be attached to the moving body 5 (senior car), and there are sensors that do not require a power cable. In addition, by attaching the display 11 to the handle 7, the detection information of the obstacle 8 can be transmitted to the user of the moving body 5 in a state in which the processing personal computer (the depression angle estimation unit 3 and the detection unit 4) is folded. . Alternatively, a transmission method using sound from the speaker 12 is also possible.

  When the power (DC 12V) necessary for the depth sensor 2 can be directly supplied from the battery 13 of the mobile body 5 (senior car), it is not necessary to place a new battery dedicated to the depth sensor 2, and the mobile body 5 of the depth sensor 2 is no longer required. Implementation becomes more realistic.

  Further, as in the example in which the depression angle estimation unit 3 and the detection unit 4 are configured by a notebook PC, the obstacle detection device 1 includes a battery and a power supply interface (for example, a universal serial bus (USB; Universal) In the case where a notebook PC having a serial bus) is provided, the depth sensor 2 may receive power from the notebook PC. Note that the present invention is not limited to such a configuration, and data such as measured images and distances may be transferred from the depth sensor 2 to the notebook PC via the interface.

(Modification)
In addition, although the example which the mobile body 5 is a senior car was shown, this invention is not limited to this. The mobile body 5 may be, for example, an unmanned mobile robot that works in an environment such as a warehouse. The present invention can be applied to an unmanned mobile robot unless the environment is entirely exposed to direct sunlight, and the mobile robot can detect obstacles and grooves on the floor surface. Here, the “environment where the entire area is directly exposed to direct sunlight” means an environment where the entire surface of the obstacle to be detected is illuminated by sunlight and cannot be shaded at all. For example, even if the entire area is directly exposed to direct sunlight, the environment where there is an obstacle such as an obstacle illuminated by the sunlight is not an “environment where the entire area is directly exposed to direct sunlight”. As described above, the obstacle detection device 1 can measure the distance data in an environment where the shade exists.

  This method can be applied not only to senior cars but also to pedestrian-controlled silver cars, electric wheelchairs, bicycles, and the like. In addition, even for people with visual disabilities, if there is some kind of transmission means such as hearing, it can be used for danger detection support during walking. As the depth sensor 2 and the data processing device are further reduced in size, the application range is expected to further expand.

(Embodiment 2)
(Algorithm when the handle may be tilted)
FIG. 9 is a diagram schematically illustrating the configuration of the obstacle detection apparatus 1a according to the second embodiment. The same components as those described in FIG. 1 are denoted by the same reference numerals. Detailed description of these components will not be repeated.

  The obstacle detection device 1a according to the second embodiment includes a depression angle / roll angle estimation unit (angle estimation means) 17 instead of the depression angle estimation unit 3 of the obstacle detection device 1 according to the first embodiment. The other configuration of the obstacle detection apparatus 1a is the same as that of the obstacle detection apparatus 1. The depression angle / roll angle estimation unit 17 estimates the depression angle of the depth sensor 2 and the roll angle of the depth sensor 2 generated according to the rotation of the handle 7 according to the rotation of the handle 7 (FIG. 2).

  FIG. 10 is a diagram for explaining the operation of the obstacle detection apparatus 1a according to the second embodiment. Next, consideration will be given to the case where the handle 7 is rotated.

In general vehicles such as senior cars, the rotational axis of the steering wheel 7 is inclined not far from the vertical but toward the near side as viewed from the driver as it goes up. For this reason, as shown in FIG. 10, when the handle 7 is rotated, the depth sensor 2 attached to the heel is inclined obliquely. That is, the depth sensor 2 after rotating the handle 7 is rotated about the roll angle ψ in the yz plane after rotating around the x axis that rotates with the rotation of the handle 7 and faces the front direction after the rotation. It can be seen that the depression angle is changing at the same time.
Therefore, with the roll angle generated by the rotation as ψ, the coordinates (x, y, z) are converted into values when the coordinate axes are rotated. First, if there is no rotation,

Furthermore, when rotating at the sensor position,

From this, when setting an optimization problem such that z is close to h,

here,

It is. Also, the angle that tilts when the handle is turned is physically restricted,

Degree.
Also, the depression angle α can be solved as a minimization problem with a constraint condition because it changes within a range of about ± 15 degrees at most with respect to the attached angle.

  It should be noted that since this coordinate system has a road surface immediately below the position of the depth sensor 2 as an origin, it is necessary to take into account that the sensor is inclined and the height changes (becomes lower). When rotated to the maximum, the sensor position is lowered by about 3 cm. Although it can be considered as an error range, if an exact value is to be calculated, h is set as a function h (ψ) of ψ, and an experiment is made in advance to determine how h changes with respect to ψ. You can give it a value such as a table.

  Algorithms that numerically solve the optimization problem represented by (Equation 12) generally have many types as nonlinear minimization problems, and need not be limited to a specific algorithm.

(Estimation of height)
An estimated value z _{ij in} the height direction when the handle 7 is rotated is obtained from (Equation 11) using the obtained estimated value of the roll angle ψ. In addition, x _ij , y _ij (obstruction 8, plane position of the groove 14 on the road surface 6) can be estimated simultaneously. Therefore, when the handle 7 is rotated, the height (obstacle 8) or the exact planar position of the groove 14 can be known.

(Embodiment 3)
The same reference numerals are given to the same components as those described in the above-described embodiment and drawings. Detailed description of these components will not be repeated.

  Unlike the above-described first and second embodiments, the algorithm according to the present embodiment estimates the depression angle α, roll angle ψ, and height h for each screen distance data acquired by the depth sensor 2. It is that. For this reason, this algorithm is based on the movement of the moving body 5 or walking of the depth sensor 2 (occurs when the road surface is bad), changes in the roll angle (occurs when the steering wheel is turned or the body tilts), high Resistant to changes in height (occurs when walking, etc.).

(Configuration of the obstacle detection device 1b)
FIG. 11 is a diagram schematically illustrating the configuration of the obstacle detection apparatus 1b according to the third embodiment. As shown in FIG. 11, the obstacle detection apparatus 1b is different from the other embodiments in that it includes a depression angle / roll angle / height estimation unit (state estimation means) 17a.

  FIG. 12 is a diagram for explaining a usage pattern of the obstacle detection apparatus 1b shown in FIG. As shown in FIG. 12, in the obstacle detection apparatus 1b, the depth sensor 2 is attached to the waist 50 of the pedestrian 5a. Here, the pedestrian 5a corresponds to the moving body 5 in the above-described embodiment. For example, when the moving body 5 is a pedestrian 5a and the depth sensor 2 is attached to the pedestrian 5a, the height h shown in FIG. However, since the algorithm of the present embodiment described later is resistant to changes such as the height h, the obstacle detection apparatus 1b can accurately detect the obstacle.

  Further, the depth sensor 2 is connected to a notebook PC indicated by 20 in FIG. The notebook PC corresponds to the portion indicated by 20 in FIG. 11, and has a configuration corresponding to the battery 13, the display 11, the speaker 12, the detection unit 4, and the depression angle / roll angle / height estimation unit 17a. . With such a configuration, the notebook PC 20 receives the information sensed by the depth sensor 2, estimates the depression angle, roll angle, and height, detects an obstacle, and detects from the configuration corresponding to the display 11 and the speaker 12. The result can be presented to the pedestrian 5a.

  In addition, in FIG. 12, although the example which attached one depth sensor 2 to the waist | hip | lumbar part 50 of the pedestrian 5a is shown, it is not necessarily limited to this structure, For example, a leg part, a chest part, an arm part of the pedestrian 5a, The depth sensor 2 may be attached to the shoulder, head, back, or the like, or a plurality of obstacle detection devices 1b may be attached to a plurality of different locations. The obstacle detection device 1b may include a plurality of depth sensors 2.

  Further, the depth sensor 2 is preferably attached so that the depression angle α in FIG. 11 is about 20-40 degrees. In order to attach the depth sensor 2 to a desired part of a moving body including a person while maintaining a depression angle α suitable for obstacle detection, the depth sensor 2 is preferably small. However, the present invention is not limited to such a configuration, and if the depression angle α suitable for obstacle detection can be ensured, the obstacle detection device 1b may be an attachment or gyroscope for stabilizing the position of the depth sensor 2. A dynamic posture stabilization control mechanism using the above may be provided.

  FIG. 12 shows the configuration using the depth sensor 2 and the notebook personal computer. However, the configuration is not limited to this configuration. For example, the depth sensor 2 and the notebook PC 20 shown in FIG. 11 may be mounted on one device. The depth sensor 2 and the like can be reduced in size. For example, all the configurations required by the obstacle detection device 1b may be mounted on one smart phone.

  FIG. 13 is a diagram for explaining another usage pattern of the obstacle detection apparatus 1b shown in FIG. As shown in FIG. 13, in the obstacle detection apparatus 1b, the depth sensor 2 is attached to the waist 50 of the pedestrian 5a as in the example shown in FIG. Then, the notebook PC 20 shown in FIG. 12 is put in the bag 10a, and the pedestrian 5a receives the obstacle detection result by voice through the connection cable 32. With this configuration, the obstacle detection device 1b that can be conveniently carried can be provided. Further, by adopting a configuration in which the user receives the detection result by voice or the like, for example, the obstacle detection result can be provided to a user who is blind. The output method of the detection result of the obstacle detection device 1b is not limited to voice, and the obstacle detection device 1b may output the detection result by vibration or the like.

(Estimation of depression angle α, roll angle ψ, height h)
The distance to the road surface at the position on each lattice point (pixel) obtained from the depth sensor 2 is theoretical from the relationship between polar coordinates and orthogonal coordinates if the depression angle α, roll angle ψ, and height h of the depth sensor 2 are known. A typical value is easily obtained. Therefore, by comparing the theoretical value with the actually measured value, it can be determined whether the point is on the road surface 6, a point higher than the road surface 6, or a low point.

  However, since the moving body 5 (senior car) swings considerably during movement, it is inevitable that the depth sensor 2 itself attached to the heel 10 also sways, and it is difficult to make the depression angle α known. In the case of a pedestrian, all α, ψ, and h are all unstable and cannot be said to be constant.

  Therefore, in the present embodiment, a method of removing the influence of a portion considered to be a point on a non-road surface, such as a robust least square method or RANSAC, is used by utilizing the fact that many points are on the road surface 6. Thus, these values are estimated (updated) by minimizing the error between the estimated height obtained from the actual measurement value and the road surface height (can be set to 0) by the unknown variables α, ψ, and h. . This estimation method is referred to as “estimation method using optimization” in this specification. By applying the estimated value, the uneven state of the road surface 6 can be detected by the method described above.

(Relationship between polar coordinate system and Cartesian coordinate system)
FIG. 14 is a diagram illustrating a situation when the depth sensor in the orthogonal coordinates and the polar coordinates rotates at the roll angle ψ. In the distance data observed at the lattice points, r _ij is the distance value at the i-th vertical and j-th horizontal positions. At this time, the relationship between orthogonal coordinates and polar coordinates can be expressed by the following equation. Here, k is an integer representing a discrete time.

The Cartesian coordinate system uses the position of the ground directly below the sensor as the origin. Polar coordinates have the sensor position as the origin. Therefore, there is a height bias between them. R _x (ψ (k)) is rotation around the x axis. That is, R _x (ψ (k)) can be expressed by the following equation.

(Estimation method using optimization)
The estimation method algorithm is an extension of the above-described “algorithm when the steering wheel is tilted” in the second embodiment.

The value of height z _ij is extracted from the above (formula 13) and can be expressed as the following formula.

If the points i and j are on the road surface, z _ij is approximately 0 as shown in the following equation.

  If the set of points on the road surface is G, these points should be almost 0. Therefore, in order to minimize the sum of squares of errors from 0, h is also set as an unknown variable to be estimated in (Equation 12). And α, ψ, and h are estimated according to the following equations.

  Since the set G on the road surface should be almost zero at these points, the unknown parameter α representing the correct posture is selected by selecting the unknown parameters α, ψ, and h so as to minimize the sum of squares of the error from 0. , Ψ, h are obtained. However, it is not known which points should be included in the set G here. Here, assuming that the change in posture is gentle compared to the sampling period, the estimated value of the posture can be considered to be in the vicinity of the immediately preceding estimated value, and includes the following steps 1 to 4. An iterative algorithm is effective.

  Step 1. Unknown parameters α, ψ, and h are set to initial values.

Step 2. Based on the assumed parameter value or the previous parameter estimation value, (Equation 15) is calculated, and a set of points (i, j) at which z _ij (k) is almost 0 is defined as G.

  Step 3. The G obtained as described above is used to solve the minimization problem represented by (Equation 16) and update the estimated value of the unknown parameter. For this minimization problem, various mathematical algorithms can be used. However, since the calculation time must be reduced, a method of applying the fast convergence Newton method once by using the Hessian matrix which is second order differential information is used. It is desirable to devise such as adopting.

  Step 4. Return to step 2.

(Embodiment 4)
The same reference numerals are given to the same components as those described in the above-described embodiment and drawings. Detailed description of these components will not be repeated.

  In Embodiment 3, the depression angle α, the roll angle ψ, and the height h, which are three unknown parameters, are estimated by an estimation method using optimization. However, the method of the third embodiment does not consider the depression angle α, the roll angle ψ, the transition speed of the height h, and the like as will be described later. However, as for unknown parameter estimation methods, these values are regarded as random variables, and dynamic models are set to estimate using nonlinear filters such as extended Kalman filter and particle filter. Is possible. Therefore, this estimation method is referred to as an estimation method using a state space model in this specification, and will be described below.

(Estimation method using state space model)
(State vector and dynamic model)
The state variable vector is set to q as follows:

  The state variable is assumed to change according to the following random walk model. This model is a model in which the posture of a moving object such as a person with a sensor is changed over time.

Where δ _kl is the Kronecker delta. That is, it is assumed that these three unknowns are uncorrelated with each other and uncorrelated in time. q _α , q _ψ , and q _h are their variances. The size per unit time that changes is determined by the length of the sampling time. For example, when worn on a person, how much the forward tilt angle changes, how much the body tilts, how much the height changes in tens of milliseconds when walking. It is expressed as a random variable, and the value can be set roughly by physically considering it.

(Observation model)
The observation model derives an equation for observing points on the road surface.If the value estimated from the equation and the actually observed value deviate significantly, the observation model is not on the road surface, and the difference is positive or negative. Or whether it is an obstacle. If the (i, j) th point comes from the road surface, z _ij (k) is almost zero. Therefore, if z _ij (k) = 0 in (Expression 15), the following nonlinear observation equation is obtained.

The function θ _i (q ₁ ) is (Equation 13). By arranging this as a vertical vector for all observed values (i, j), an observation model can be constructed.

  A non-linear state space model can be constructed by (Expression 17) and (Expression 18). Assuming that an error is added to (Equation 18), the observation equation can be expressed as the following equation.

  A nonlinear Kalman filter can be used for the above model. In that case, when using an extended Kalman filter, it is necessary to obtain Jacobian.

(State estimation algorithm)
The observation values include data coming from places other than the road surface, which can be determined using a threshold value. Thus, it is necessary to apply the extended Kalman filter after excluding the points treated as points other than the road surface from the observation vector.

(Details of state estimation)
(When observing the road surface)
The observed value r _ij and the true value f _ij can be expressed as follows:

Here, e _ij ^(g) (k) represents a measurement error. The measurement error includes an actual sensor noise based on the accuracy limit of the specification and an error based on a geometric measurement difference between the observation model and the actual sensor property.

probability density function of ^{_{e ij (g) (k)}} (PDF; Probability Density Function) p g (e ij (g) (k)) , the average is 0, the following equation as a Gaussian distribution of variance sigma _g ² Can be expressed.

(When observing obstacles)
Since the distance from the depth sensor to the obstacle cannot be specified, the observed value r _ij can be expressed as the following equation.

Here, the probability density function p _s (e _ij ^(s) ) of e _ij ^(s) (k) can be expressed by the following equation, for example.

(When observing grooves or recesses)
This is the opposite of observing obstacles. The observed value rij can be expressed as the following equation.

Here, the probability density function p _l (e _ij ^(l) ) of e _ij ^(l) (k) can be expressed by the following equation, for example.

Here, r ^* represents a measurement limit distance and is about 10 m.

(Probability density function model)
Based on the PDF defined as described above, a mixed PDF (mixture PDF) can be expressed as follows.

Here, the second term on the right side shows the case where the observed distance is “short”, and the third term shows the case where the observed distance is “long”. The prior probability P _s should be set based largely on the amount of obstacles in the observation field of view, and P _l should be zero if there are no grooves or recesses.

  Since the observation model is non-linear, there are several candidates for the state estimation scheme. Here, an extended Kalman filter is applied. According to this, compared with a particle filter or other state estimation schemes, the calculation load can be further reduced.

(Handling of mixed PDF)
Next, handling of the mixed PDF will be described. When an obstacle, hole, or recess is the observation source, the observation value becomes an abnormal value, so a simple approach is to delete the observation data generated from the obstacle, hole, or recess (possibly). Can be cited as one of In the following, an approach according to this method will be described.

(Observation source estimation)
The probability when the observation source is a road surface is P _g , the probability when it is an obstacle is P _s, and the probability when it is a groove or a recess is P _l . After observing r _ij (k), these probabilities can be described as posterior probabilities:

Here, R ^{1: k} = {r (1),. . . , R (k)} (where r is a vector), mε {g, s, l}, and rij (k) is any other subscript other than (i, j). Is assumed to be independent of the value of. P m and _(c ij _(k)), when the prior probability, probability density function p _g when observed source is road surface can be expressed by the following equation.

Further, the probability density function p _s when the observation source is an obstacle can be expressed as the following equation.

Here, the estimated value of H _ij (k | k−1) (with “^”) can be expressed as the following equation.

In addition, the estimated value of f _ij (k | k−1) (with “^”) can be expressed as the following equation.

Here, g (k) can be expressed as the following equation.

Then, the probability density function p _l where observation source is a groove or recess can be expressed by the following equation.

Regarding these PDFs, the following relationship will be described. First, the following relationship holds between the probability when the observation source is the road surface and the probability when the observation source is an obstacle. Here, r ^* is the above-mentioned maximum measurement limit distance.

Moreover, the relationship of following Formula is established between the probability when an observation source is a road surface, and the probability when it is a groove | channel or a recessed part.

Then, the observed value r _ij (k) is a value of a section composed of predetermined constant values c ₁ (k) and c ₂ (k) as in the following equation.

(Handling detected objects)
Hereinafter, a method of handling when the observation source of the observation value r _ij (k) is determined to be an object at a level above the road surface or a place at a level below the road surface will be described.

P (c _ij (k) = g | R ^{1: k} ) <P (c _ij (k) = s | R ^{1: k} ) or P (c _ij (k) = g | R ^{1: k} ) <P ( When c _ij (k) = l | R ^{1: k} ), it can be considered that the observation source of the observed value r _ij (k) is not a road surface.

In (Equation 18), since it is assumed that the observed source of observations _r ij (k) is road surface, _r ij (k) of such a case, or much smaller than the true value _{f ij} Can be bigger. In other words, in order to estimate an unknown state variable from the observation data, it is necessary to suppress the influence of the observation value that becomes such an abnormal value. This can be said to be a robust estimation problem. The inventors have developed a technique for simultaneously estimating states and unknown system parameters in a linear system (M. Tanaka and T. Katayama: Identification of a Linear System with Switching Parameters by the EM Algorithm. , Systems, Control and Information, Vol.1, No.2, pp. 68-76, 1988. (hereinafter, Tanaka1), “M. Tanaka and T. Katayama: A Robust Identification of a Linear System with Outliers by The EM Algorithm, Systems, Control and Information, Vol. 1, No. 4, pp. 117-126, 1988 ”(hereinafter referred to as reference Tanaka 2)). The above problem also includes unknown parameters (such as covariance). However, since the problem itself is complicated, simplification of the problem is promoted by focusing on the state estimation scheme.

  As the inventors have demonstrated in the document Tanaka 1, the state can be estimated based on a non-nominal problem. However, it is considered that the data is not enough to perform state estimation in the same way as the method in the document Tanaka1, so as described in the document Tanaka2, an approach to exclude data judged to have been observed from non-nominal observation sources Is adopted.

In the following, the approach will be described in more detail. First, it is assumed that the observed value r _{i * j *} (k) is determined to be an abnormal value in some (i, j) pairs. Then, not only the nonlinear function f _{i * j *} (k) but also the observed value is excluded from the observation model represented by (Equation 19). The element of (i ^* , j ^* ) is excluded from the observation vector, and the row of (i ^* , j ^* ) is excluded from the matrix H. In the following, the matrix after the exclusion is expressed by adding “*” to the upper right of the symbol representing the matrix.

(Details of algorithm)
Hereinafter, the details of the iterative calculation in the algorithm of this embodiment will be described.

  1. Set the initial value as follows.

  2. The time is updated as follows:

  3. Update the observed values as follows:

Here, c _ij (k) representing the observation source is estimated as follows.

The estimated value of the state q (with “^”) and the error matrix P are updated as follows.

The matrix or vector to which “*” is attached in the above mathematical formula corresponds to I _ij (k) represented by the following formula where a specific row or column is 0 in the matrix or vector of the original size. Here, I (k) is a diagonal matrix composed of 1 or 0 elements represented by the following equation.

(Parameter value)
Parameters need to be defined based on real systems. When the depth sensor of the present invention is attached to a pedestrian, parameters are set as follows.

In (Equation 20), when the initial value of the state q (with “−”) is [α ₀ ψ ₀ h ₀ ] ^T , when the depth sensor of the present invention is mounted on a human body belt, α ₀ is 20 [°] × π / 180 [rad]. Here, the notation enclosed in [] represents the unit of a numerical value. Also, ψ ₀ is 0 [rad]. Further, h ₀ is the height of the depth sensor when a person stands up (for example, 800 [mm]).

Identification of covariance is more difficult. By considering the amount of movement in one measurement period, the uncertainty of the dynamics of the system can be specified. In the present invention, the frame rate is 10 [fps]. That is, one measurement cycle is 100 [ms]. Here, the variation of α is, for example, 5 [°]. If this value is treated as a standard deviation, q _α = (5π / 180) ² for the variance. Similarly, q _ψ = (5π / 180) ² . Similarly, q _h = 20 ² [mm ² ].

  Observation noise is due to lens distortion. From an empirical rule, in the present invention, the observation noise is defined as follows.

Here, σ ² = 10 ² .

As described above, the depression angle / roll angle / height estimation unit 17a is a non-linear structure configured by equations (17) and (18) based on the relationship between the distance data, the depression angle, the roll angle, and the height. For the state space model, the depression angle, the roll angle, and the height are estimated while allowing variation based on the extended Kalman filter for obtaining the optimum Kalman gain K ^* as described above.

(Experimental result)
In the experimental results shown below, P _g = 0.59, P _s = 0.3, P _l = 0.1, q _α = 2, q _ψ = 0.5, and q _h = 1000.

(Static experiment)
FIG. 15 is a diagram illustrating an image during obstacle detection in the system according to the fourth embodiment. As shown in FIG. 15, in the system according to the present embodiment, two video windows are displayed. The large window on the right displays a camera image with the detection results superimposed. Observation points determined to be higher than the road surface are indicated by a predetermined color. The observation point determined to be at a position lower than the road surface is indicated by another color. The observation points where the distance value cannot be used are indicated by other colors.

  The image shown in FIG. 15 is an image illuminated by a considerable amount of direct sunlight because the present invention is implemented in a corridor near the window on a sunny day, and is illuminated by the sunlight. Distance values are no longer available at certain observation points. However, even in such a case, the obstacle can be sufficiently detected.

  Next, the meaning of numerical values in the video will be described. The numerical value of 0.68 on the left indicates the ratio of the road surface in the sample data (vertically and horizontally every 8 points). The numerical value of 0.56 on the right indicates the ratio of the road surface area at all observation points (640 × 480). On the second line, four numerical values are displayed. The numerical value on the left (11 fps) is the current frame rate. The second numerical value (37.5 deg) from the left is an estimated value of the depression angle α. In this experiment, it is attached to a belt (wound around a human body), and this result is reasonable. The third numerical value (−1.0 deg) is the roll angle. The numerical value (853.9 mm) displayed on the right is the height at the position of the depth sensor.

  In the vertically long window on the left, the road surface state is displayed on the plane map. This state consists of 8 × 3 blocks. The central area shows a state in the range of 15 cm on the left and right. The left area shows a state in the range from 15 cm to 50 cm, and the right area shows a state in the same range.

(Dynamic experiment)
16 (a) to 16 (c) show estimated values of the depression angle α, the roll angle ψ, and the height h when the depth sensor according to this embodiment is attached to the scooter (see FIG. 17) and the scooter travels in the corridor. FIG.

  As shown in FIGS. 16A to 16C, the estimated value is stable except for the central portion of the time axis. This is because the scooter driver rotated the steering wheel at the time corresponding to the central portion of the time axis. As shown in FIG. 16B, the roll angle ψ varies greatly depending on the structure of the scooter. At the same time, as shown in FIG. 16 (c), the center (h) of the handle is lowered during rotation. Furthermore, the depression angle α is large as shown in FIG. These are adapted to the actual situation.

  The estimated value of the height h is about 10 cm smaller than the actual value, but the cause is largely based on the structure of the lens system.

  When the algorithm in the estimation method using the state space model according to the present embodiment is applied, the transition speed (parameters (α, ψ, h)) is between every data acquisition time (for example, several tens of milliseconds). The amount of change) can be controlled by a model that updates the above-mentioned state, etc., so that even when obstacles that are large in the height direction such as walls and stairs are included in the observation field of view, obstacles etc. can be detected correctly can do. In addition, in the model for updating the state, if the variance is large, the model can allow the state to change greatly for each update. Therefore, with the configuration according to the present embodiment, it is possible to estimate the depression angle, roll angle, and height of the depth sensor at the moment when the depth sensor acquires the distance image, and the depression angle, roll angle, Even if the distance fluctuates, an obstacle on the road surface on which the mobile body travels can be detected.

(Summary)
In this embodiment, the state space model of the data measured by the depth sensor is configured so that the state vector includes three unknown parameters representing the posture of the moving object. Real-time estimation scheme is realized by applying extended Kalman filter. This is realized by removing observation points other than the road surface from the observation points in the state estimation scheme. Originally, a depth sensor cannot acquire data under direct sunlight, but even in such an environment, a dangerous situation such as an obstacle or a groove can be detected relatively correctly. This is because data is obtained in the shadow of the object.

  The present invention is not limited to the above-described embodiments, and various modifications are possible within the scope shown in the claims, and embodiments obtained by appropriately combining technical means disclosed in different embodiments. Is also included in the technical scope of the present invention.

  INDUSTRIAL APPLICABILITY The present invention can be used for an obstacle detection apparatus including a depth sensor that is attached to a moving body such as a vehicle or a person and acquires a road surface distance image. In addition, the applicability is wide as a tool for the visually impaired and elderly people to go out at night. Furthermore, the present invention can be applied to walking assistance for visually impaired persons.

DESCRIPTION OF SYMBOLS 1, 1a, 1b Obstacle detection apparatus 2 Depth sensor 3 Depression angle estimation part (angle estimation means)
4 Detection part (detection means)
5 Moving objects (vehicles, pedestrians)
6 Road surface 7 Handle 8 Obstacle 9 Horizontal line 10 駕 籠 11 Display 12 Speaker 13 Battery 14 Groove 15 Side surface 16 Upper surface 17 Depression angle / roll angle estimation unit (angle estimation means)
17a Inclination angle / roll angle / height estimation unit (state estimation means)
L1, L2 line C1, C2 line α 俯 angle h height r distance ψ roll angle

Claims (7)

A depth sensor attached to the vehicle to obtain a distance image of the road surface;
An angle estimating means for estimating a depression angle of the depth sensor attached to the vehicle based on the distance image acquired by the depth sensor;
Detection means for detecting an obstacle present on the road surface based on the depression angle of the depth sensor estimated by the angle estimation means, the height of the depth sensor from the road surface, and the distance data of the distance image. An obstacle detection device characterized by that.   The obstacle detection device according to claim 1, wherein the angle estimation unit estimates the depression angle by minimizing an error based on a least square method that eliminates the influence of a point on a non-road surface. The vehicle has a handle;
The obstacle detection device according to claim 1, wherein the angle estimation unit estimates the depression angle and a roll angle of the depth sensor generated according to the rotation of the handle according to the rotation of the handle.   The obstacle detection apparatus according to claim 1, wherein the depth sensor is a non-scanning infrared laser sensor. A depth sensor attached to a pedestrian to obtain a distance image of the road surface;
Based on the distance image acquired by the depth sensor, state estimation means for estimating the depression angle, roll angle, and height of the depth sensor attached to the pedestrian,
Detecting means for detecting an obstacle present on the road surface based on the depression angle, roll angle, and height of the depth sensor estimated by the state estimating means and the distance data of the distance image; Obstacle detection device.   The obstacle detection according to claim 5, wherein the state estimation unit estimates the depression angle, the roll angle, and the height by minimizing an error based on a least square method that removes the influence of a point on a non-road surface. apparatus.   The state estimation means allows variation based on a nonlinear Kalman filter that removes the influence of points on a non-road surface with respect to the state space model based on the relationship between the distance data, the depression angle, the roll angle, and the height. The obstacle detection device according to claim 5, wherein the depression angle, the roll angle, and the height are estimated.