JP2008298742A - Posture angle detection device, posture angle detecting method, and surface of floor detection device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To detect a posture angle or a plane on which it travels, without spending cost. <P>SOLUTION: In a posture angle detection device, a distance computing part 28 calculates a distance Lm<SB>(i)</SB>into a plurality of points on a surface of floor based on a reflected light received a light receiving device 20. A posture detection device 30 detects the posture angle θ, based on each distance Lm<SB>(i)</SB>calculated from output of each light receiving element of the light receiving device 20, and each theoretical distance Lr<SB>(θ')</SB>from a distance measuring device 12 to the surface of floor at each case of the plurality of the candidate posture angle θ'. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a posture angle detection device, a posture angle detection method, and a floor surface detection device.

Conventionally, a time-of-interest that measures the distance to an object based on the time from when light modulated at a predetermined frequency is emitted to the object until the reflected light from the object is received by the light receiving unit. A flight-type distance image sensor is known. Also, an obstacle detection device that uses this distance image sensor to measure the distance to a plane on which a moving body travels or an observation object is known (for example, see Patent Document 1). The distance image sensor is mounted on the movable body at a predetermined height and obliquely downward with a predetermined posture angle (predetermined inclination). In this obstacle detection device, a distance image of a plane corresponding to each posture angle that varies due to vibration or the like during traveling of the moving body is stored in the storage means as a candidate reference image. Thereby, even when the attitude angle varies, the measured distance image is compared with a plurality of candidate reference images, and the traveling plane is detected by using the candidate reference image having the highest degree of coincidence. Can do.
JP 2006-262009 A

  However, in the obstacle detection device described in Patent Literature 1, it is necessary to prepare candidate reference images for each posture angle in which fluctuation is predicted in the roll direction and the pitch direction. For example, if the range of predicted posture angle fluctuation is ± 10 ° in the roll direction and ± 20 ° in the pitch direction and stored in the storage means every 1 °, 20 × 40 = 800 candidate reference images are stored. It is necessary to memorize the means. However, in order to store as many as 800 candidate reference images, a storage means having a large capacity is required. If such a storage means having a large capacity is used, the cost is increased. Therefore, the obstacle detection device described in Patent Document 1 has a problem that it is expensive to detect a traveling plane when the posture angle varies.

  The present invention has been made to solve the above-described problems, and does not cost much, and the attitude angle detecting device, the attitude angle detecting method, and the floor surface detecting device for detecting the traveling plane are detected. The purpose is to provide.

  In order to achieve the above object, a posture angle detection device according to a first aspect of the present invention includes a light source that emits modulated light onto a floor surface, and a plurality of light receiving elements that receive reflected light of light emitted from the light source. A measuring unit provided with a light receiving unit arranged at a predetermined height from the floor so as to have a predetermined posture angle with respect to the floor; and reflected light received by the light receiving unit Based on distance calculation means for calculating distances to a plurality of points on the floor, each distance calculated from the output of each light receiving element of the light receiving means, and each of a plurality of candidate posture angles Posture angle detection means for detecting the posture angle based on each theoretical distance from the measurement means to the floor surface.

  According to the attitude angle detection device of the first invention, the distance calculating means calculates the distances to a plurality of points on the floor surface based on the reflected light received by the light receiving means. The attitude angle detecting means is based on each distance calculated from the output of each light receiving element of the light receiving means and each theoretical distance from the measuring means to the floor in each case of a plurality of candidate attitude angles. Detect posture angle. Therefore, according to the posture angle detection apparatus according to the first aspect of the present invention, the posture angle can be detected without requiring a storage unit having a large capacity. Therefore, the posture angle detection device according to the first aspect of the invention can detect the posture angle without cost.

  In order to achieve the above object, an attitude angle detection device according to a second aspect of the present invention includes a light source that emits modulated light onto a floor surface, and a plurality of light receiving elements that receive reflected light of the light emitted from the light source. A measuring unit provided with a light receiving unit arranged at a predetermined height from the floor so as to have a predetermined posture angle with respect to the floor; and reflected light received by the light receiving unit A distance calculating means for calculating distances to a plurality of points on the floor surface, an approximate plane determined based on each distance calculated from the output of each light receiving element of the light receiving means, and a plurality of candidate postures And a posture angle detecting means for detecting the posture angle based on each theoretical plane in each case of the corner.

  According to the attitude angle detection device of the second invention, the distance calculation means calculates distances to a plurality of points on the floor surface based on the reflected light received by the light receiving means. The attitude angle detection means is based on an approximate plane determined based on each distance calculated from the output of each light receiving element of the light receiving means, and each theoretical plane in each case of a plurality of candidate attitude angles. Detect posture angle. Therefore, according to the posture angle detection device according to the second invention, particularly when the range occupied by the obstacle on the floor surface is small or when there is no obstacle on the floor surface, the storage means having a large capacity is stored. It is possible to detect the posture angle without the need for. Therefore, the posture angle detection device according to the second invention can detect the posture angle without cost.

  Further, the distance calculation means in the posture angle detection device according to the first or second aspect of the invention provides information indicating a predetermined distance when the amount of reflected light received by the light receiving means is less than or equal to a predetermined amount. It can be calculated. Thereby, the information indicating the predetermined distance is, for example, information indicating that the measurement of the distance is impossible, so that the floor surface exists at a distance larger than the distance at which the light travels in one cycle at the predetermined frequency. Can be calculated as information indicating that it is impossible to measure the distance, so that an error in measuring the distance can be prevented. The information indicating the predetermined distance is, for example, one cycle of light at a predetermined frequency and a distance to the floor surface determined according to the time from when the light is irradiated from the light source to when the reflected light is received by the light receiving means. By using the information indicating the distance obtained by adding the distance traveled to the distance to the floor surface, the distance to the floor surface existing at a distance greater than the distance traveled by light at a predetermined frequency can be accurately calculated. Measurement errors can be prevented.

  In order to achieve the above object, a floor surface detection device according to a third invention is detected by the distance measurement device according to the first invention or the distance measurement device according to the second invention, and the posture angle detection means. And a floor surface detecting means for detecting the floor surface based on the posture angle. As a result, the floor surface can be detected without requiring a storage means having a large capacity. Therefore, the floor surface detection apparatus according to the third aspect of the invention can detect the floor surface without cost.

  In order to achieve the above object, a posture angle detection method according to a fourth aspect of the present invention includes a light source that emits modulated light onto a floor surface, and a plurality of light receiving elements that receive reflected light of the light emitted from the light source. The light receiving means of the measuring means provided with the arranged light receiving means and provided at a predetermined height position with respect to the floor surface from the floor surface receives the reflected light, and Based on the reflected light received by the light receiving means, the distance to a plurality of points on the floor surface is calculated, and each distance calculated from the output of each light receiving element of the light receiving means and a plurality of candidate attitude angles The posture angle is detected based on each theoretical distance from the measurement means to the floor surface in each case.

  According to the attitude angle detection method of the fourth invention, the distance to a plurality of points on the floor surface is calculated based on the reflected light received by the light receiving means, and is calculated from the output of each light receiving element of the light receiving means. The posture angle is detected based on each of the distances and the theoretical distances from the measuring means to the floor in each case of a plurality of candidate posture angles. Therefore, according to the posture angle detection method according to the fourth aspect of the present invention, it is possible to detect the posture angle without requiring a storage unit having a large capacity. Therefore, the posture angle detection method according to the fourth aspect of the invention can detect the posture angle without cost.

  In order to achieve the above object, a posture angle detection method according to a fifth aspect of the present invention includes a light source that emits modulated light on a floor surface, and a plurality of light receiving elements that receive reflected light of light emitted from the light source. The light receiving means of the measuring means provided with the arranged light receiving means and provided at a predetermined height position with respect to the floor surface from the floor surface receives the reflected light, and Based on the reflected light received by the light receiving means, the distance to a plurality of points on the floor surface is calculated, and an approximate plane determined based on each distance calculated from the output of each light receiving element of the light receiving means; The posture angle is detected based on each theoretical plane in each case of a plurality of candidate posture angles.

  According to the attitude angle detection method of the fifth aspect of the invention, the distances to a plurality of points on the floor surface are calculated based on the reflected light received by the light receiving means, and are calculated from the outputs of the respective light receiving elements of the light receiving means. The posture angle is detected based on the approximate plane determined based on each distance and the theoretical plane in each case of a plurality of candidate posture angles. Therefore, according to the posture angle detection method according to the fifth aspect of the present invention, the posture angle can be detected without requiring a storage unit with a large capacity. Therefore, the posture angle detection method according to the fifth aspect of the invention can detect the posture angle without cost.

  As described above, according to the posture angle detection device, the posture angle detection method, and the floor surface detection device of the present invention, it is possible to detect the posture angle or the floor surface without cost. It is done.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

  As shown in FIG. 1, the floor surface detection device 1 according to the embodiment of the present invention includes a posture angle detection device 10 and a traveling body 16. The attitude angle detection device 10 includes a distance measurement device 12 and a control device 13. The posture angle detection device 10 is mounted on a traveling body 16 that travels on a floor surface.

  As shown in FIG. 2, the distance measuring device 12 is configured such that an angle formed between the optical axis 70 of the light emitted from the light source 18 and the horizontal direction at a predetermined height H from the floor surface is a predetermined posture angle θ, For example, it is installed with a posture angle of 30 °.

  As shown in FIG. 3, the distance measuring device 12 includes a distance calculating device 15, a light source 18, and a light receiving device 20.

  As illustrated in FIG. 4, the light source 18 includes a plurality of, for example, 50 to 60 LEDs that irradiate light arranged in a matrix. Each LED emits light whose intensity is modulated by a sine wave having a predetermined frequency, for example, 20 MHz, under the control of the light source control unit 22.

  The light receiving device 20 includes a plurality of light receiving elements. For example, the light receiving device 20 includes a CMOS sensor including a plurality of pixels. The light receiving device 20 receives reflected light from the object 22 or the floor surface. The light receiving device 20 outputs a detection signal based on the amount of received reflected light.

  The distance calculation device 15 is a ROM (not shown) as a storage medium storing a program for executing various processing routines, a CPU (not shown) for reading and executing the program from the ROM, and temporarily storing data. The microcomputer includes a RAM (not shown) and an I / O (input / output) port (not shown). The distance calculation device 15 includes a light source control unit 22, a flight time calculation unit 24, a received light amount detection unit 26, and a distance calculation unit 28.

  The light source control unit 22 controls the light source 18 so as to emit modulated light whose intensity is modulated with a sine wave having a predetermined frequency, for example, 20 MHz. Further, the light source control unit 22 outputs a signal indicating the control timing of the light source 18.

  The time-of-flight calculation unit 24 detects a phase shift between the light emitted from the light source 18 and the reflected light received by the light receiving device 20 based on a detection signal from the light receiving device 20. Then, based on the detected phase shift, the time-of-flight calculation unit 24 calculates the time from when the light from the light source 18 is irradiated until the reflected light is received by the light receiving device 20.

  The received light amount detection unit 26 detects the received light amount of the reflected light received by the light receiving device 20.

  The distance calculation unit 28 performs a reference received light amount calculation process and a distance calculation process.

In the reference light reception amount calculation process, the distance calculation unit 28 calculates a temporary distance to the floor surface or the object 22 for each light receiving element of the light receiving device 20 based on the time calculated by the flight time calculation unit 24. Then, as shown in FIG. 5, the distance calculation unit 28 is the maximum value of the amount of light received by the light receiving element whose calculated distance is not more than a predetermined value, for example, 0.3 m or less. The reflectance r is calculated by the following formula (1) using Rmax and the distance X1 calculated at that time.
Rmax = r / (X1) ² Equation (1)

Then, the distance calculation unit 28 uses the calculated reflectance r and the maximum measurement distance Xmax (7.5 m in the present embodiment) unique to the distance measurement device 12, according to the following equation (2): The reflected light intensity R at the maximum measurement distance Xmax is calculated. The maximum measurement distance Xmax is a value unique to the distance measurement device 12 and is determined based on the frequency of light emitted from the light source 18 and the like.
R = r / (Xmax) ² Equation (2)

  Then, the distance calculation unit 28 calculates R calculated by Expression (2) as a reference received light amount Rref used in the distance calculation process.

  In the distance calculation process, the distance calculation unit 28 determines the floor surface based on the time calculated by the time-of-flight calculation unit 24 for a light receiving element whose received light amount detected by the received light amount detection unit 26 is larger than the reference received light amount Rref. Alternatively, the distance to the object 22 is calculated. On the other hand, the distance calculation unit 28 calculates information indicating that the distance cannot be measured as information indicating the distance to the floor surface or the object 22 for a light receiving element having a light reception amount equal to or less than the reference light reception amount Rref. .

  Conventionally, as shown in FIG. 6A, an object 22 existing at a distance exceeding the maximum measurement distance Xmax (7.5 m in the present embodiment), for example, an object 22 existing at a distance of 8.5 m. Was measured incorrectly as 1.0 m. This is because even if the phase is shifted more than one wavelength, the time of flight of light is calculated by regarding this phase shift as a phase shift within one wavelength. When the distance calculation unit 28 performs the reference light reception amount calculation process and the distance calculation process as in the present embodiment, as illustrated in FIG. 6B, the object 22 existing at a position exceeding the maximum measurement distance Xmax. With respect to, by calculating information indicating that distance measurement is impossible as information indicating distance, erroneous measurement is eliminated.

  In the distance calculation process, for a light receiving element whose received light amount is equal to or smaller than the reference received light amount Rref, as shown in FIG. 6C, a floor surface or an object determined according to the time calculated by the flight time calculating unit 24 Information indicating the distance to the floor surface or the object 22 may be calculated by adding the distance that the light travels in one cycle at a predetermined frequency to the distance up to 22. By doing so, the object 22 existing at a distance exceeding the maximum measurement distance Xmax (7.5 m in the present embodiment), for example, the object 22 existing at a distance of 8.5 m is 8.5 m ( = 1.0 m + 7.5 m) can be accurately measured.

  The control device 14 includes a ROM (not shown) as a storage medium that stores a program that executes a posture detection processing routine, a program that executes various processing routines, and a CPU (not shown) that reads and executes the program from the ROM. ), A RAM (not shown) for temporarily storing data, and a microcomputer including an I / O (input / output) port (not shown). The control device 14 includes an attitude detection unit 30, an installation condition storage unit 32, and a theoretical distance calculation unit 34.

  The installation condition storage unit 32 stores a plurality of candidate posture angles θ ′. This candidate posture angle θ ′ is a predetermined posture angle θ (a predetermined posture angle θ () in the traveling direction (pitch direction) of the moving body 16 due to the vibration applied to the distance measuring device 12 when the traveling body 16 travels. In the present embodiment, for example, the attitude angle θ of the distance measuring device 12 that is expected when it deviates from θ = 30 ° is represented. For example, in this embodiment, when the attitude angle of the distance measuring device 12 is set to 30 ° in advance, it is assumed that the attitude angle is expected to be shifted by ± 5 ° at the maximum when the traveling body 16 travels. At this time, the installation condition storage unit 32 stores a candidate posture angle θ ′ from 25 ° to 35 °, for example, every 1 °. Further, the installation condition storage unit 32 stores a height H from the floor surface to the distance measuring device 12.

  The theoretical distance calculation unit 34 uses the height H and the candidate posture angle θ ′ stored in the installation condition storage unit 32, and the distance when the posture angle θ of the distance measuring device 12 is each candidate posture angle θ ′. A theoretical distance (theoretical distance) on the optical axis 70 of the light source 18 from the measuring device 12 to the floor surface is calculated.

  The posture detection unit 30 calculates the distance corresponding to each of the N light receiving elements among the distances of the plurality of light receiving elements of the light receiving device 20 calculated by the distance calculation unit 28 and the theoretical distance calculation unit 34. The posture angle θ of the distance measuring device 12 is detected based on the theoretical distance for each candidate posture angle θ ′.

  The traveling body 16 includes a floor surface detection unit 36 that detects a floor surface.

  The floor surface detection unit 36 detects the floor surface using the distance for each light receiving element calculated by the distance calculation unit 28, the posture angle θ detected by the posture detection unit 30, and the like. And the floor surface detection part 36 detects the obstruction with respect to the detected floor surface. And the floor surface detection part 36 controls the driving | running | working (for example, moving speed and moving direction) of the traveling body 16 so that it may correspond to the detected obstruction.

  Next, a posture detection processing routine performed by the CPU of the control device 14 will be described with reference to FIG. In the present embodiment, this posture detection processing routine is executed every time the traveling body 16 travels a predetermined distance based on a signal from the floor surface detection unit 36.

First, in step 100, the posture detection unit 30, among the distances to the floor surfaces of the plurality of light receiving elements of the CMOS sensor of the light receiving device 20 calculated by the distance calculation unit 28, as shown in FIG. A distance Lm _(i) (i = 1, 2,..., N) from each of the light receiving elements to the floor surface m _(i) (i = 1, 2,..., N) is acquired. Here, the N light receiving elements may be selected by any method, and among the plurality of light receiving elements of the light receiving device 20, N light receiving elements may be selected so as to be equally spaced. It should be noted that θm _(i) (i = 1, 2,..., N) illustrated in the figure is the light reflected from each of the floor surfaces m _(i) and reaching the light receiving device 20 (P). Represents the angle formed with the floor surface.

In the next step 102, the theoretical distance calculation unit 34 determines the posture angle of the distance measuring device 12 from each candidate posture angle θ ′ and the height H stored in the installation condition storage unit 32 as shown in FIG. 9. Theoretical distance Lr _{(θ ′)} (θ ′ = 25, 26,..., 35) on the optical axis 70 of the light source 18 from the distance measuring device 12 to the floor surface when θ is each candidate posture angle θ ′. Is calculated by the following equation (3).

Lr _{(θ ′)} = H / sin θ ′ (3)
In FIG. 9, for example, when the candidate posture angle θ ′ is 25 ° (A), the candidate posture angle θ ′ is 26 ° (B), and the candidate posture angle θ ′ is 35 ° (C The theoretical distances Lr ₍₂₅₎ , Lr ₍₂₆₎ , and Lr _{(35) in} FIG.

As a result, the theoretical distance Lr _{(θ ′)} is calculated for each of the plurality of candidate posture angles θ ′.

In the next step 104, the posture angle detection unit 30 calculates the theoretical distance Lr _{(θ ′)} (θ ′ = 25, 26,..., 35) calculated in step 102 and the distance Lm _(i) acquired in step 100. Each of (i = 1, 2,..., N) is compared to create a histogram based on the comparison result. Specifically, the posture angle detection unit 30 includes N differences indicating the difference in absolute value between the theoretical distance Lr _{(θ ′)} and each of the plurality of distances Lm _(i) at each candidate posture angle θ ′. Is calculated. Then, the posture angle detection unit 30 calculates a score of θ ′ = 25 to 35, with the number of calculated N differences equal to or less than the predetermined threshold Q as the score of the candidate posture angle θ ′. (That is, the number of θ ′ = 25 to 35 is calculated by setting the number satisfying | Lm _(i) −Lr _{(θ ′)} | ≦ Q as the number of candidate posture angles θ ′.)

  A histogram as shown in FIG. 10 is created by the processing in steps 100 to 104 described above. This histogram indicates that the candidate posture angle θ ′ is more likely to be the actual posture angle θ as the number of candidate posture angles θ ′ increases.

  In the next step 106, the posture angle detection unit 30 detects the candidate posture angle θ ′ having the maximum score as the current posture angle θ of the distance measuring device 12.

  When the histogram as shown in FIG. 10 is created, the candidate posture angle θ ′ having the maximum score is 29 °, and therefore the posture angle detection unit 30 sets 29 ° to the current distance measuring device 12. Is detected as the posture angle θ of the.

  Through the above attitude angle detection process, the actual attitude angle θ of the distance measuring device 12 is detected.

As described above, according to the attitude angle detection device 10 of the present embodiment, the distance calculation unit 28 is based on the reflected light received by the light receiving device 20 and is a distance Lm to a plurality of points on the floor surface. _(I) is calculated. The posture detection unit 30 is a theory from the distance measurement device 12 to the floor surface in each case of each distance Lm _(i) calculated from the output of each light receiving element of the light receiving device 20 and a plurality of candidate posture angles θ ′. The posture angle θ is detected based on each of the distances Lr _{(θ ′)} above. Therefore, according to the posture angle detection apparatus 10 of the present embodiment, the posture angle θ can be detected without requiring a storage unit having a large capacity. Therefore, the posture angle detection apparatus 10 of the present embodiment can detect the posture angle without cost.

  In addition, in the posture angle detection device 10 according to the present embodiment, when the light reception amount detected by the light reception amount detection unit 26 is equal to or less than the reference light reception amount Rref, light from the posture angle detection device 10 is 1 at a predetermined frequency. The distance to the floor surface or the object 22 existing at a distance greater than or equal to the distance to advance in the cycle is calculated as information indicating that the distance cannot be measured. Therefore, according to the attitude angle detection device 10 of the present embodiment, errors in distance measurement can be prevented.

Note that the posture angle detection processing routine described above is not limited to Step 104 and Step 106 described above. For example, in step 104 and step 106, the posture detection unit 30 adds the absolute value of the difference between the theoretical distance Lr _{(θ ′)} and the plurality of distances Lm _(i) (Σ | Lr _{( )} for each candidate posture angle θ ′. _{θ ′) −} Lm _(i) |) is created, and the candidate posture angle θ ′ that minimizes the sum of the absolute values of the differences between the theoretical distance Lr _{(θ ′)} and the plurality of distances Lm _(i) It may be detected as the posture angle θ of the distance measuring device 12.

  Further, the posture angle detection processing routine described above is not limited to step 106 described above. The posture angle θ detected in step 106 described above is an integer value, but the posture angle θ may be detected by calculating the center of gravity of the histogram created in step 104. Thereby, the posture angle θ may be a decimal value.

  Further, the posture angle detection processing routine described above is not limited to the above-described step 102 to step 106. For example, in steps 102 to 106, an approximate plane equation obtained by using a multivariate analysis method such as the least square method based on the distances of the N light receiving elements acquired in step 100, and a plurality of candidates Compare each theoretical plane expression that represents the floor that can be calculated from each of the attitude angles θ ′, and detect the candidate attitude angle θ ′ in the theoretical plane expression that has the highest degree of coincidence as the attitude angle θ. You may make it do. As a result, the posture angle can be detected particularly when the range occupied by the obstacle on the floor surface is small or when there is no obstacle on the floor surface.

  In the above-described posture angle detection processing routine, an example in which the posture angle θ is detected when the posture angle θ is expected to be shifted in the pitch direction has been described. However, the present invention is not limited to this. . For example, when it is predicted that the posture angle θ is shifted in the roll direction perpendicular to the pitch direction in the horizontal plane, the posture angle θ may be detected based on the same principle.

  Further, the reference received light amount calculation process and the distance calculation process performed by the distance calculation unit 28 described above are not limited to the above-described contents, and may be performed as follows. For example, in the reference received light amount calculation process, the distance calculation unit 28 calculates a temporary distance to the floor surface or the object 22 for each light receiving element of the light receiving device 20 based on the time calculated by the flight time calculation unit 24. To do. Then, the distance calculation unit 28 uses the average value of the received light amount received by the light receiving element whose calculated distance is less than a predetermined value, for example, 0.3 m or less, in the calculated temporary distance, in the distance calculation process. Calculate as a quantity. Note that the reference light reception amount may be a value obtained by dividing the sum of the maximum value and the minimum value of the light reception amount received by the light receiving element whose distance of 0.3 m or less is calculated, divided by two. In the distance calculation process, the distance calculation unit 28 calculates the calculated distance as the distance to the floor surface or the object 22 for the light receiving element whose distance calculated in the reference received light amount calculation process is greater than 0.3 m. For a light receiving element having a distance of 0.3 m or less calculated in the reference received light amount calculation process, if the received light amount detected by the received light amount detecting unit 26 is larger than the reference received light amount, the flight time calculating unit 24 Based on the time calculated in step 1, the distance to the object 22 is calculated. On the other hand, when the received light amount detected by the received light amount detection unit 26 is a light receiving element equal to or less than the reference received light amount, the distance calculating unit 28 displays information indicating that the distance cannot be measured up to the object 22. Calculated as information indicating the distance.

  In the present embodiment, the example in which the attitude angle θ of the distance measuring device 12 is an angle formed by the optical axis 70 of the light emitted from the light source 18 and the horizontal direction has been described. It may be an angle formed by the optical axis 70 of light and the vertical direction. At this time, sin θ used in the above description can be used as sin (90−θ).

It is a figure which shows the arrangement position of the floor surface detection apparatus in this Embodiment. It is a figure which shows the arrangement position of the distance measuring device in this Embodiment. It is the schematic which shows this Embodiment. It is a figure for demonstrating the light source in this Embodiment. It is a figure which shows the relationship between distance and reflected light intensity. It is a figure which shows the effect of this Embodiment. It is a figure which shows the flowchart of the attitude | position detection process routine which CPU of the control apparatus in this Embodiment performs. It is a figure which shows the irradiation range of the light from the light source in this Embodiment. It is a figure for demonstrating the theoretical distance in this Embodiment. It is a figure which shows the histogram of the candidate attitude | position angle in this Embodiment.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Floor surface detection apparatus 10 Attitude angle detection apparatus 12 Distance measurement apparatus 14 Control apparatus 15 Distance calculation apparatus 18 Light source 20 Light reception apparatus 22 Light source control part 24 Flight time calculation part 26 Light reception amount detection part 28 Distance calculation part 30 Attitude detection part 32 Installation Condition storage unit 34 Theoretical distance calculation unit

Claims (8)

A light source that emits modulated light on the floor surface, and a light receiving unit in which a plurality of light receiving elements that receive reflected light of the light emitted from the light source are arranged, and the light source is arranged at a predetermined height from the floor surface. A measuring means provided to have a predetermined posture angle with respect to the floor surface;
Distance calculating means for calculating distances to a plurality of points on the floor surface based on the reflected light received by the light receiving means;
Based on the respective distances calculated from the outputs of the respective light receiving elements of the light receiving means and the theoretical distances from the measuring means to the floor in each case of a plurality of candidate posture angles Attitude angle detection means for detecting the angle;
A posture angle detection device including: A light source that emits modulated light on the floor surface, and a light receiving unit in which a plurality of light receiving elements that receive reflected light of the light emitted from the light source are arranged, and the light source is arranged at a predetermined height from the floor surface. A measuring means provided to have a predetermined posture angle with respect to the floor surface;
Distance calculating means for calculating distances to a plurality of points on the floor surface based on the reflected light received by the light receiving means;
The posture angle is detected based on an approximate plane determined based on each distance calculated from the output of each light receiving element of the light receiving means and each theoretical plane in each case of a plurality of candidate posture angles. Attitude angle detecting means for performing,
A posture angle detection device including:   The posture angle detection device according to claim 1, wherein the distance calculation unit calculates information indicating a predetermined distance when the amount of reflected light received by the light receiving unit is a predetermined amount or less. .   The posture angle detection device according to claim 3, wherein the information indicating the predetermined distance is information indicating that the distance cannot be measured.   The information indicating the predetermined distance includes the distance to the floor surface determined according to the time from when the light from the light source is irradiated until the reflected light is received by the light receiving means, and the light at the predetermined frequency. The posture angle detection device according to claim 3, which is information indicating a distance obtained by adding a distance advanced in one cycle. The attitude angle detection device according to any one of claims 1 to 5,
Floor surface detection means for detecting a floor surface based on the posture angle detected by the posture angle detection means;
Including a floor surface detection device. A light source that emits modulated light on the floor surface, and a light receiving unit in which a plurality of light receiving elements that receive reflected light of the light emitted from the light source are arranged, and the light source is arranged at a predetermined height from the floor surface. The light receiving means of the measuring means provided to have a predetermined posture angle with respect to the floor surface receives the reflected light,
Based on the reflected light received by the light receiving means, calculate the distance to a plurality of points on the floor surface,
Based on the respective distances calculated from the outputs of the respective light receiving elements of the light receiving means and the theoretical distances from the measuring means to the floor in each case of a plurality of candidate posture angles Attitude angle detection method that detects angles. A light source that emits modulated light on the floor surface, and a light receiving unit in which a plurality of light receiving elements that receive reflected light of the light emitted from the light source are arranged, and the light source is arranged at a predetermined height from the floor surface. The light receiving means of the measuring means provided to have a predetermined posture angle with respect to the floor surface receives the reflected light,
Based on the reflected light received by the light receiving means, calculate the distance to a plurality of points on the floor surface,
The posture angle is detected based on an approximate plane determined based on each distance calculated from the output of each light receiving element of the light receiving means and each theoretical plane in each case of a plurality of candidate posture angles. Yes Posture angle detection method.

Images