JP6030405B2 - Planar detection device and autonomous mobile device including the same - Google Patents
Planar detection device and autonomous mobile device including the same Download PDFInfo
- Publication number
- JP6030405B2 JP6030405B2 JP2012235950A JP2012235950A JP6030405B2 JP 6030405 B2 JP6030405 B2 JP 6030405B2 JP 2012235950 A JP2012235950 A JP 2012235950A JP 2012235950 A JP2012235950 A JP 2012235950A JP 6030405 B2 JP6030405 B2 JP 6030405B2
- Authority
- JP
- Japan
- Prior art keywords
- plane
- distance image
- axis
- distance
- projection image
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Manipulator (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Description
本発明は、距離画像から平面を検出する平面検出装置、およびそれを備えた自律移動装置に関する。 The present invention relates to a plane detection device that detects a plane from a distance image, and an autonomous mobile device including the same.
ロボットや無人搬送車といった自律移動装置においては、移動時に前方にある障害物や段差を検出し、衝突や転落を防止する必要がある。このような障害物や段差の検出には、赤外線方式や超音波方式の近接センサ広く用いられていた。 In an autonomous mobile device such as a robot or an automated guided vehicle, it is necessary to detect obstacles and steps ahead and prevent collisions and falls when moving. Infrared or ultrasonic proximity sensors have been widely used for detecting such obstacles and steps.
しかし、赤外線方式や超音波方式の近接センサは、前方の障害物の有無は判別できるが、その詳細な位置や形状までは得られない。したがって、ロボットに取り付けた場合、直近に存在する障害物や段差などの検出はできるものの、進行方向前方の広い範囲の障害物や段差を予め発見して回避しながら移動するような用途には用いることができない。そこで、レーザレンジファインダ（ＬＲＦ）などの距離センサが、用いられるようになった。 However, an infrared or ultrasonic proximity sensor can determine the presence or absence of a front obstacle, but cannot obtain its detailed position and shape. Therefore, when it is attached to a robot, it can detect obstacles and steps that are present in the immediate vicinity, but it is used for applications that move while finding and avoiding obstacles and steps in a wide range ahead in the direction of travel. I can't. Therefore, distance sensors such as a laser range finder (LRF) have come to be used.
図１３は、従来技術におけるレーザレンジファインダ１０１を備えた自律移動装置１００の図であり、図１３中の（ａ）は、従来技術におけるレーザレンジファインダ１０１を備えた自律移動装置１００の外観図を示す図であり、図１３中の（ｂ）は、従来技術におけるレーザレンジファインダ１０１を備えた自律移動装置１００が検出できない上方の障害物１０２、下方の障害物１０３、および段差１０４の図である。図１３中の（ａ）に示すように、自律移動装置１００は、レーザレンジファインダ１０１を高さＨＬＲＦの位置に備えている。レーザレンジファインダ１０１は、高さＨＬＲＦの位置において、水平方向（図のｙ軸に垂直な面内）に角度スキャンを行い、障害物、および段差を検出する。
FIG. 13 is a diagram of the autonomous
しかし、レーザレンジファインダ１０１は、計測精度は高いが、計測する高さと異なる位置にある障害物、および段差は検出できないという欠点がある。例えば、図１３中の（ｂ）において、レーザレンジファインダ１０１は、障害物１０２、障害物１０３、および段差１０４を検出することはできない。したがって、障害物１０２、障害物１０３、および段差１０４を検出するために、レーザレンジファインダ１０１の他に、赤外線方式の近接センサ、超音波方式の近接センサなどを多数配置する必要があり、自律移動装置が煩雑な構造となっていた。
However, although the
そこで、障害物、および段差を検出するために、障害物、および段差が含まれる画像を取得し、その画像から平面を検出して、その平面上の障害物、および段差を検出するという手法が提案されている。特許文献１においては、ステレオカメラを用いて、左右画像の微分画像のＨｏｕｇｈ変換結果から道路面を検出し、それを手掛かりに他の車両や歩行者などを検出する。 Therefore, in order to detect an obstacle and a step, there is a method of acquiring an image including the obstacle and the step, detecting a plane from the image, and detecting the obstacle and the step on the plane. Proposed. In Patent Document 1, a road surface is detected from a Hough transform result of a differential image of left and right images using a stereo camera, and other vehicles, pedestrians, and the like are detected using this as a clue.
また、特許文献２においては、ＴＯＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）方式の距離画像センサによって得られる距離画像データから、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）法と呼ばれる計算手法によって平面情報を抽出し、それを手掛かりに歩行者などを検出する。 In Patent Document 2, plane information is extracted by a calculation method called a RANSAC (RANdom Sample Consensus) method from distance image data obtained by a distance image sensor of a TOF (Time Of Flight) method, and walking is performed using the information as a clue. Detect people.
しかしながら、前記特許文献１に示された方法においては、ステレオカメラが必須である上、道路上の白線や路面端など、平面を検出するための分かりやすい手掛かりが必要となる。例えば、特許文献１にかかる平面検出装置を移動式ロボットに備えた場合、移動式ロボットが走行する路面上に白線や路面端などの分かりやすい手掛かりがない場合には、左右画像の対応関係が得られないため適用できない。 However, in the method disclosed in Patent Document 1, a stereo camera is essential, and easy-to-understand clues for detecting a plane such as a white line on a road and a road surface end are required. For example, when the mobile robot is equipped with the flat surface detection apparatus according to Patent Document 1, if there is no easy-to-understand clue such as a white line or a road surface end on the road surface on which the mobile robot travels, the correspondence relationship between the left and right images is obtained. Not applicable.
また、前記特許文献２に示された方法においては、距離画像データから直接平面情報を抽出できるものの、距離画像データ内に平面とは無関係の情報が多数含まれると、それがノイズとなって平面検出の障害となる。特に、距離画像データ内の大部分が平面以外の情報で占められているような場合には、平面が検出できなくなってしまう。また、平面を特定する際に算出すべきパラメータが多いため、演算時間が増大するという問題がある。 Further, in the method disclosed in Patent Document 2, plane information can be extracted directly from distance image data. However, if a lot of information unrelated to a plane is included in the distance image data, it becomes noise and the plane information is extracted. This is an obstacle to detection. In particular, when most of the distance image data is occupied by information other than the plane, the plane cannot be detected. In addition, since there are many parameters to be calculated when specifying the plane, there is a problem that the calculation time increases.
本発明は、前記の問題点を鑑みてなされたものであり、その目的は、距離画像からその画像内に含まれる平面を、高速かつより確実に検出できる平面検出装置を提供することにある。 The present invention has been made in view of the above-described problems, and an object of the present invention is to provide a flat surface detection apparatus that can detect a plane included in the image from a distance image at high speed and more reliably.
前記の課題を解決するために、本発明に係る平面検出装置は、特定の検出対象平面を含む被写体の距離画像データから当該特定の検出対象平面を検出する平面検出装置であって、前記距離画像データを、前記特定の検出対象平面を表す検出対象３次元点群を含む３次元座標データに変換する３次元座標演算手段と、前記３次元座標データを所定の２次元平面に投影して、前記検出対象３次元点群が線形に分布した投影画像データを生成する投影画像生成手段と、前記投影画像データから前記線形の直線を検出する直線検出手段と、前記直線検出手段の検出結果に基づいて、前記特定の検出対象平面の傾きに関する情報を含む平面パラメータを算出する平面パラメータ算出手段と、を備えることを特徴としている。 In order to solve the above-described problem, a flat surface detection device according to the present invention is a flat surface detection device that detects a specific detection target plane from distance image data of a subject including the specific detection target plane, and the distance image Three-dimensional coordinate calculation means for converting data into three-dimensional coordinate data including a detection target three-dimensional point group representing the specific detection target plane, and projecting the three-dimensional coordinate data onto a predetermined two-dimensional plane, Based on the detection result of the projection image generation means for generating the projection image data in which the three-dimensional point cloud to be detected is linearly distributed, the straight line detection means for detecting the linear straight line from the projection image data, and the detection result of the straight line detection means. And plane parameter calculation means for calculating a plane parameter including information on the inclination of the specific detection target plane.
また、前記の課題を解決するために、本発明に係る自律移動装置は、前記平面検出装置と、前記距離画像データを生成する距離画像生成手段と、走行手段と、を備えており、前記平面検出装置を用いて走行路となる平面を検出することを特徴としている。 In order to solve the above-mentioned problem, the autonomous mobile device according to the present invention includes the plane detection device, a distance image generation unit that generates the distance image data, and a travel unit, and the plane The present invention is characterized in that a detection plane is used to detect a plane serving as a travel path.
本発明によれば、距離画像を３次元座標データに変換し、変換した３次元座標データを所定の２次元面（平面）に投影する。そして、投影した画像から直線を検出し、平面を検出する。したがって、距離画像の中に平面検出の手掛かりを必要とせず、また障害物等の平面とは無関係の情報が距離画像に多数含まれていても当該無関係の情報を排除して平面をより確実に検出できるという効果を奏する。 According to the present invention, a distance image is converted into three-dimensional coordinate data, and the converted three-dimensional coordinate data is projected onto a predetermined two-dimensional surface (plane). Then, a straight line is detected from the projected image, and a plane is detected. Therefore, it does not require a clue for plane detection in the distance image, and even if the distance image contains a lot of information irrelevant to the plane, such as obstacles, the irrelevant information is excluded and the plane is more reliably detected. There is an effect that it can be detected.
〔実施形態１〕
以下、本発明の平面検出装置および自律移動装置の一実施形態として、平面検出装置を備えた清掃ロボットを挙げて、これを図１〜図８を用いて説明する。
Embodiment 1
Hereinafter, as an embodiment of the flat surface detection device and the autonomous mobile device of the present invention, a cleaning robot provided with the flat surface detection device will be described, and this will be described with reference to FIGS.
図１は、本実施形態１における清掃ロボット１を示す図であり、図１中の（ａ）は、この清掃ロボット１の外観図を示す図であり、図１中の（ｂ）は、その清掃ロボット１の筐体１１の内部構成を図示した断面図である。本実施形態１における清掃ロボット１は、床面を自律走行しながら清掃を行なう自律走行型の清掃ロボットである。この清掃ロボット１は、距離画像センサが取得した距離画像から平面を検出する平面検出装置を必須構成としている。清掃ロボット１は、平面検出装置によって、平面を検出し、進行方向の障害物、および段差を判別して、これらを避けて走行することができる。
FIG. 1 is a diagram showing a cleaning robot 1 according to the first embodiment. (A) in FIG. 1 is an external view of the cleaning robot 1, and (b) in FIG. 2 is a cross-sectional view illustrating an internal configuration of a
なお、本発明に係る平面検出装置は、距離画像センサを構成要素として具備してもよいし、本実施形態１において以下で説明するように、清掃ロボット１に距離画像センサおよび平面検出装置が設けられており、当該距離画像センサが取得した距離画像を平面検出装置が入手して平面を検出する態様であってもよい。本実施形態１では、距離画像センサは平面検出装置の構成要素ではなく、平面検出装置の外部構成として距離画像センサが在る態様について説明する。 Note that the flat surface detection apparatus according to the present invention may include a distance image sensor as a component, and the cleaning robot 1 is provided with the distance image sensor and the flat surface detection apparatus as described below in the first embodiment. It is also possible to adopt a mode in which the plane detection device acquires the range image acquired by the range image sensor and detects the plane. In the first embodiment, the distance image sensor is not a constituent element of the flat surface detection device, and an aspect in which the distance image sensor is provided as an external configuration of the flat surface detection device will be described.
（清掃ロボット１の構成）
図１中の（ａ）において、清掃ロボット１は、窓２１が設けられた筐体１１と、駆動輪２（走行手段）と、保護部材１２とを備えている。筐体１１の内部には、後述する種々の制御系および駆動系などが実装されており、駆動輪２が駆動制御されることによって、清掃ロボット１が走行路面を走行し、走行中あるいは走行停止中に走行路面の清掃を行なう。
(Configuration of cleaning robot 1)
In FIG. 1A, the cleaning robot 1 includes a
清掃ロボット１のより具体的な構成を図１中の（ｂ）を用いて説明する。図１中の（ｂ）において、清掃ロボット１は、窓２１が設けられた筐体１１の内部に、バッテリ４と、廃液回収ユニット４５と、洗浄液吐出ユニット４６と、モータ１０と、距離画像センサ２０（距離画像生成手段）と、演算装置３０とを実装している。更に、図１中の（ｂ）において、清掃ロボット１は、筐体１１の外部、より具体的には筐体１１と走行路面との間に、先に説明した駆動輪２とともに、従輪３と、清掃ブラシ９と、保護部材１２とを備えている。以下に各構成について説明するが、本実施形態１の特徴的構成は演算装置３０の一部に設けられた平面検出装置６０にある。そこで、以下ではこの特徴的構成について重点的に説明する一方、特徴的構成以外の構成については従来周知の構成によって実現することも可能であるため詳細な説明は省略する。
A more specific configuration of the cleaning robot 1 will be described with reference to FIG. 1B, the cleaning robot 1 includes a battery 4, a waste
なお、図１において、清掃ロボット１は、紙面左方向へ前進、紙面右方向へ後退、紙面奥、または手前方向への旋回が可能であるが、以下の説明では、主な進行方向である紙面左方向への移動を単に進行方向と記載することがある。 In FIG. 1, the cleaning robot 1 can move forward in the left direction of the paper, move backward in the right direction of the paper, turn in the rear of the paper surface, or turn in the front direction. Movement in the left direction may be simply referred to as a traveling direction.
駆動輪２は、清掃ロボット１の底部の左右に配置されており、駆動モータ（不図示）により制御されて清掃ロボット１の移動を実現する。従輪３は、清掃ロボット１の底部に回転自在に取り付けられている。駆動輪２および従輪３は、前進、後退、旋回、および停止ができ、これらの組み合わせにより清掃ロボット１の自在な走行を可能としている。
The drive wheels 2 are arranged on the left and right of the bottom of the cleaning robot 1 and are controlled by a drive motor (not shown) to realize the movement of the cleaning robot 1. The
バッテリ４は清掃ロボット１に電源を供給する。バッテリ４は、よく知られた降圧回路と整流平滑回路とにより充電され、所定の電圧を出力する。 The battery 4 supplies power to the cleaning robot 1. The battery 4 is charged by a well-known step-down circuit and a rectifying / smoothing circuit, and outputs a predetermined voltage.
洗浄液吐出ユニット４６は、洗浄液タンク５と、洗浄液吐出部６とを有している。洗浄液タンク５は、洗浄液を貯留する。また、洗浄液吐出部６は、洗浄液タンク５とパイプにより連結されており、洗浄液タンク５に貯留されている洗浄液を吐出する。
The cleaning
廃液回収ユニット４５は、廃液タンク７と、吸引口８とを有している。廃液タンク７は、清掃ロボット１が内部に吸い込んだ廃液（塵や埃等を含む）を溜める。清掃ロボット１は、吸引口８から廃液を吸い込み、吸引口８とパイプにより連結されている廃液タンク７に廃液を排出する。
The waste
清掃ブラシ９は、吸引口８の付近に設置され、洗浄液吐出部６から吐出された洗浄を使い清掃する。清掃ブラシ９は、モータ１０によって駆動される。
The cleaning brush 9 is installed in the vicinity of the
保護部材１２は、洗浄液の飛散、および異物の巻き込みを防止するために、清掃ロボット１の底部における進行方向前方側に設置されている。
The
距離画像センサ２０は、近距離用の距離画像センサ２０ａと、遠距離用の距離画像センサ２０ｂとを有している。なお、近距離用の距離画像センサ２０ａと、遠距離用の距離画像センサ２０ｂとに共通する構成について説明する場合には単に距離画像センサ２０と記載することがある。
The
距離画像センサ２０は、赤外光投射方式の距離画像センサであり、その内部に赤外光の投射素子を含む投射光学系と、赤外光の撮像素子を含む撮像光学系を有する。所定のパターンを有する赤外光を外部に投影照射し、外部物体からの反射光を撮像素子で撮影することで、撮像光学系の視野範囲内にある物体までの距離を計測することができる。近距離用の距離画像センサ２０ａと、遠距離用の距離画像センサ２０ｂとは、筐体１１の内部に配設されており、赤外光を筐体１１の窓２１を通じて外部に投射するとともに、窓２１を通じて外部から反射光を入射する。
The
距離画像センサ２０の距離計測結果は、視野範囲に含まれる物体までの距離を画像上の画素のグレースケール値として表現した距離画像（深度画像、デプス画像）として出力される。本実施形態における距離画像センサ２０ａ、および距離画像センサ２０ｂの詳細については、後述する。
The distance measurement result of the
演算装置３０は、距離画像センサ２０の距離画像を取得して、平面を検出する処理を行う。演算装置３０の構成および機能の詳細については、後述する。
The
なお、清掃ロボット１には、前記の各構成の他にも、後述する構成も含む。その他にも、手動走行、または自動走行を選択する操作パネル、手動走行時の走行方向を決定する走行スイッチ、および非常時に運転を停止させる非常停止スイッチ等の制御スイッチ５０（図４）などを備えることができる。また、清掃ロボット１の形態は、前記のような洗浄液を使用して洗浄するタイプに限定されるものではなく、ファン、集塵室、吸込口などを備えたいわゆる家庭用掃除機のような態様のロボットであってもよい。 The cleaning robot 1 includes configurations described later in addition to the above-described configurations. In addition, an operation panel for selecting manual travel or automatic travel, a travel switch for determining a travel direction during manual travel, and a control switch 50 (FIG. 4) such as an emergency stop switch for stopping operation in an emergency are provided. be able to. Moreover, the form of the cleaning robot 1 is not limited to the type of cleaning using the cleaning liquid as described above, and is an aspect like a so-called home vacuum cleaner provided with a fan, a dust collection chamber, a suction port, and the like. It may be a robot.
本発明に係る自律移動装置は、本実施形態１の清掃ロボット１でいうところの距離画像センサ２０と、後述する演算装置３０の平面検出装置６０とを必須構成としている。そこで、以下では、先述した距離画像センサ２０の詳細を説明するとともに、演算装置３０の詳細を説明する。
The autonomous mobile device according to the present invention includes a
（距離画像センサ２０の詳細）
図２は、本発明の実施形態１に係る清掃ロボット１の、距離画像センサ２０の取り付け位置と、距離画像センサ２０の計測範囲を示す図である。距離画像センサ２０は、清掃ロボット１の進行方向前面に、清掃対象である床面（走行路面）から垂直に所定の距離離間した高さ位置に取り付けられている。より具体的には、距離画像センサ２０の光軸は、進行方向前後に沿って伸びているものの、光軸の一端である撮像素子から進行方向に向かって斜め下向きになるよう、つまり距離画像センサ２０は斜め下向きに床面を向いて取り付けられている。本実施形態１においては、近距離用の距離画像センサ２０ａは、床面からの高さ（床面から撮像素子の設置位置までの高さ）Ｈ＝７４０［ｍｍ］、床面に対する光軸の角度θ＝６７．５［ｄｅｇ］となるよう取り付けられている。同様に、遠距離用の距離画像センサ２０ｂは、床面からの高さ（床面から撮像素子の設置位置までの高さ）Ｈ＝７１０［ｍｍ］、床面に対する光軸の角度θ＝２２．５［ｄｅｇ］となるよう取り付けられている。
(Details of the distance image sensor 20)
FIG. 2 is a diagram illustrating the attachment position of the
図３中の（ａ）は、遠距離用の距離画像センサ２０ｂにより撮影されたＲＧＢ画像であり、図３中の（ｂ）は、遠距離用の距離画像センサ２０ｂにより撮影された距離画像であり、図３中の（ｃ）は、近距離用の距離画像センサ２０ａにより撮影されたＲＧＢ画像であり、図３中の（ｄ）は、近距離用の距離画像センサ２０ａにより撮影された距離画像である。ＲＧＢ画像である図３中の（ａ）および（ｃ）の視野内にある物体への距離を、近い距離は明るく、遠い距離は暗く表示した距離画像が図３中の（ｂ）および（ｄ）となる。図３中の（ｂ）および（ｄ）が示すように、距離画像センサ２０ａと距離画像センサ２０ｂとは、取り付けられた位置、および水平面に対する角度が異なるため、平面である床面の角度が異なっている。
3A is an RGB image photographed by the long-distance
ここで、距離画像センサ２０の光軸を床面に対して平行に配置した場合、清掃ロボット１本体の近傍が、距離画像センサの画角から外れてしまう。したがって、清掃ロボット１本体の近距離の広い範囲が、視野外領域となり、計測不能になってしまう。これに対し、進行方向前方の斜め下向きに取り付けると、近傍の視野外領域を小さくできるので、清掃ロボット１本体の比較的近距離まで計測可能となる。
Here, when the optical axis of the
また、床面上に投影した近距離用の距離画像センサ２０ａの視野範囲は、図２に示すＡ０Ｂ０Ｃ０Ｄ０の台形領域になる。一方、遠距離用の距離画像センサ２０ｂの視野範囲は、図２のＡ１Ｂ１Ｃ１Ｄ１の台形領域になる。このように、水平面に対して異なる角度になるよう取り付けられた複数の距離画像センサ２０を併用することにより、清掃ロボット１は、清掃ロボット１の進行方向の近傍から遠方まで、広い計測範囲を持つことが可能となる。
Further, the visual field range of the short-distance
なお、距離画像センサ２０の配置や個数は本実施形態１の構成に限定されず、例えば距離画像センサ２０を１個のみ搭載したり、複数個を水平方向に並べて配置したりすることも可能である。また、本実施形態１において、近距離用の距離画像センサ２０ａおよび遠距離用の距離画像センサ２０ｂは、同一波長の赤外光源を用いているため、互いの干渉を防ぐ目的で、視野領域Ａ０Ｂ０Ｃ０Ｄ０とＡ１Ｂ１Ｃ１Ｄ１との間に図２に示すようにわずかに隙間を設けている。異なる波長の光源を用いるなどして干渉を防ぐことが可能であれば、２つの視野領域の間に隙間を設けないように近距離用の距離画像センサ２０ａおよび遠距離用の距離画像センサ２０ｂを設置することも可能である。
The arrangement and the number of the
（清掃ロボット１の走行機能）
図４は、本実施形態１の清掃ロボット１における、走行機能に関連する構成を示すブロック図である。図４に示すように、清掃ロボット１は、先に説明した距離画像センサ２０および演算装置３０の他に、走行制御部４１、清掃制御部４２、マップ情報メモリ部４３、状態表示部４４、ロータリーエンコーダ４７、駆動輪モータ４８、ジャイロセンサ４９、および制御スイッチ５０を備えている。
(Running function of cleaning robot 1)
FIG. 4 is a block diagram illustrating a configuration related to the travel function in the cleaning robot 1 of the first embodiment. As shown in FIG. 4, the cleaning robot 1 includes a travel control unit 41, a
演算装置３０は、距離画像センサ２０から距離画像を取得し、取得した距離画像から障害物や段差の位置、大きさ、および形状を抽出する。抽出された障害物、および段差の情報（以下、障害物・段差データと称する）は走行制御部４１に出力される。演算装置３０の詳細は後述する。
The
走行制御部４１は、駆動輪２に取り付けられたロータリーエンコーダ４７、およびジャイロセンサ４９の情報に基づき、清掃ロボット１の移動距離、および現在の位置と方向とを把握する。予めマップ情報メモリ部４３に保存されたマップ情報、および演算装置３０から出力された障害物・段差データに基づき、障害物や段差を回避するように走行経路を決定し、駆動輪モータ４８を制御する。また、制御スイッチ５０から信号を取得すると、それに応じて非常停止や走行方向の変更など必要な制御を行う。これらの制御に関する情報は状態表示部４４に表示され、リアルタイムに更新される。
The traveling control unit 41 grasps the moving distance of the cleaning robot 1 and the current position and direction based on information from the
清掃制御部４２は、走行制御部４１からのコマンドを受けて、清掃ブラシ９、廃液回収ユニット４５、および洗浄液吐出ユニット４６の動作開始、および停止の切り替えなど、清掃に関する部分を制御する。
The
マップ情報メモリ部４３は、清掃ロボット１が清掃する範囲の障害物、および段差などの情報が保存されている。マップ情報メモリ部４３に保存されている情報は、走行制御部４１により、更新される。
The map
状態表示部４４は、清掃ロボット１の状態に関する情報を表示する。例えば、手動走行、または自動走行の表示、非常時の運転停止の表示等である。
The
ロータリーエンコーダ４７は、駆動輪２に取り付けられており、回転の変位をデジタル信号として走行制御部４１に出力する。ロータリーエンコーダ４７の出力により、走行制御部４１は、進んだ距離を把握することができる。
The
ジャイロセンサ４９は、向きの変化を検出し、走行制御部４１に出力する。ジャイロセンサ４９の出力により、走行制御部４１は進行方向を把握することができる。 The gyro sensor 49 detects a change in direction and outputs the change to the traveling control unit 41. From the output of the gyro sensor 49, the traveling control unit 41 can grasp the traveling direction.
（演算装置３０の詳細）
図４に示すように、演算装置３０は、平面検出装置６０、障害物・段差検出部３５、およびデータ統合部３６を備えている。平面検出装置６０は、図４に示すように、３次元座標演算部３１（３次元座標演算手段、第２の３次元座標演算手段）と、投影画像生成部３２（投影画像生成手段、第２の投影画像生成手段）と、直線検出部３３（直線検出手段、第２の直線検出手段）と、平面検出部３４（平面パラメータ算出手段、第２の平面パラメータ算出手段）とを有している。
(Details of the arithmetic unit 30)
As illustrated in FIG. 4, the
・３次元座標演算部３１
３次元座標演算部３１は、距離画像センサ２０から距離画像を取得し、取得した距離画像を３次元座標データに変換する。３次元座標データの座標系の定義を、図５を用いて説明する。
・ 3D coordinate
The three-dimensional coordinate
図５中の（ａ）は、近距離用の距離画像センサ２０ａ基準の３次元座標系を示す図であり、図５中の（ｂ）は、遠距離用の距離画像センサ２０ｂ基準の３次元座標系を示す図であり、図５中の（ｃ）は、清掃ロボット１基準の３次元座標系を示す図である。なお、近距離用または遠距離用の距離画像センサ２０ａ、２０ｂ基準の３次元座標系とは、距離画像センサ２０の光学中心を原点として、進行方向を向いたときの左右方向をｘ軸（右向きを正）、上下方向をｙ軸（上向きを正）、前後方向、即ち距離画像センサ２０の光軸方向をｚ軸（奥行き方向を正）にとる。距離画像センサ２０ａおよび距離画像センサ２０ｂは取り付け位置、および角度が異なるため、図５中の（ａ）および（ｂ）に示すように、座標系も互いに異なっている。また、距離画像センサ２０基準の座標系で表した距離と、清掃ロボット１本体から床面に沿って計測した距離とも異なる。したがって、清掃ロボット１から対象物までの正確な距離を求めるためには、清掃ロボット１基準（床面基準）の座標系に座標変換して、２つの距離画像センサのデータを統合する必要がある。
(A) in FIG. 5 is a diagram showing a three-dimensional coordinate system based on the
そこで、図５中の（ｃ）に示すように、距離画像センサ２０基準のｘｙｚ座標とは別に、清掃ロボット１基準の座標系であるＸＹＺ座標を定義する。進行方向をＺ軸、床面の法線方向をＹ軸、Ｚ軸およびＹ軸に垂直な方向をＸ軸（右向きを正）とする。
Therefore, as shown in (c) of FIG. 5, XYZ coordinates that are a coordinate system based on the cleaning robot 1 are defined separately from the xyz coordinates based on the
なお、本実施形態１においては、距離画像センサ２０基準のｘ軸の向きと、清掃ロボット１基準のＸ軸の向きとが、ほぼ一致している。これは即ち、距離画像センサ２０がｚ軸を中心に回転する方向に傾いて取り付けられてはおらず、床面と画像センサ２０との傾きは、ｘ軸を中心に回転する方向の傾きθだけであるということを意味する。もしくは、ｚ軸を中心に回転する方向に傾いていたとしても、傾きθに比べて十分小さく、無視できるということを意味する。
In the first embodiment, the x-axis direction based on the
続いて、３次元座標データに含まれる点の座標を算出する方法について説明する。３次元座標（ｘ，ｙ，ｚ）データのうち、ｚ座標は距離画像に含まれる距離そのものである。ｘ座標、およびｙ座標は、距離画像センサ２０の光学系の焦点距離ｆ、画素ピッチｐ、光軸と撮像素子中心との画素ズレ量ｃなどが分かれば、三角測量の原理によりｚから計算することが出来る。本実施形態１においては、事前に距離画像センサに対しキャリブレーションを行い、これらのパラメータを求めておく。これにより、距離画像が得られれば、その画像に含まれる全画素において距離画像センサ２０基準の３次元座標（ｘ，ｙ，ｚ）データ、即ち３次元点群（Ｐｏｉｎｔ Ｃｌｏｕｄ）データを得ることができる。
Next, a method for calculating the coordinates of points included in the three-dimensional coordinate data will be described. Among the three-dimensional coordinate (x, y, z) data, the z coordinate is the distance itself included in the distance image. The x-coordinate and y-coordinate are calculated from z based on the principle of triangulation if the focal length f of the optical system of the
図６は、本実施形態１において、３次元座標データの例を示す図である。前述したように、進行方向に向いて左右方向をｘ軸、上下方向をｙ軸、前後方向をｚ軸にとっている。 FIG. 6 is a diagram illustrating an example of three-dimensional coordinate data in the first embodiment. As described above, in the traveling direction, the left-right direction is the x-axis, the up-down direction is the y-axis, and the front-back direction is the z-axis.
なお、３次元座標演算部３１は、前記の座標系以外にも、例えばｘ軸、ｙ軸、およびｚ軸のうち、少なくとも一つの軸を中心に回転させた座標系や、原点を変更した座標系など、様々な座標系において、距離画像を３次元座標データに変換可能である。
In addition to the coordinate system described above, the three-dimensional coordinate
・投影画像生成部３２
投影画像生成部３２は、３次元座標データを、２次元面（平面）に投影した投影画像を生成する。例えば、ｘｙ平面に投影された投影画像は、全ての画素について、そのｘ座標とｙ座標とを抽出したものになる。同様に、ｙｚ平面に投影された投影画像は、全ての画素について、そのｙ座標とｚ座標とを抽出したものとなり、ｚｘ平面に投影された投影画像は、全ての画像について、そのｚ座標とｘ座標とを抽出したものとなる。
Projected
The projection
ここで、投影画像は白（＝「１」）／黒（＝「０」）の２値の画像であり、その縦横のサイズは、投影時の縮尺に応じて自由に決めることができる。例えば、ｙｚ平面への投影画像の条件として、ｙ軸の投影範囲を−１２００ｍｍ〜＋１２００ｍｍ（オフセット１２００ｍｍ）、ｚ軸の投影範囲を０ｍｍ〜＋３２００ｍｍ（オフセット０ｍｍ）、投影時の縮尺を１／１０［ｐｉｘｅｌ／ｍｍ］とする。この場合、投影画像のサイズは以下のようになる。 Here, the projected image is a binary image of white (= “1”) / black (= “0”), and the vertical and horizontal sizes can be freely determined according to the scale at the time of projection. For example, as a condition of a projected image on the yz plane, the projection range of the y axis is −1200 mm to +1200 mm (offset 1200 mm), the projection range of the z axis is 0 mm to +3200 mm (offset 0 mm), and the scale at the time of projection is 1/10 [ pixel / mm]. In this case, the size of the projected image is as follows.
縦（ｙ軸）： （１２００−（−１２００））／１０＝２４０
横（ｚ軸）： （３２００−０）／１０＝３２０
即ち、横３２０ｐｉｘｅｌ×縦２４０ｐｉｘｅｌとなる。
Vertical (y-axis): (1200 − (− 1200)) / 10 = 240
Horizontal (z-axis): (3200-0) / 10 = 320
That is, the horizontal 320 pixels × vertical 240 pixels.
この投影画像サイズは、原画像（距離画像）の画像サイズとは独立に、上記等の縮尺を変えることで自由に変更することができる。投影画像サイズを大きくすると、投影時の分解能が細かくなることで計算の精度が上がるが、その分演算時間が長くなる。このトレードオフによって実際に使用する画像サイズが決定される。 This projected image size can be freely changed by changing the scale as described above, independently of the image size of the original image (distance image). When the projection image size is increased, the resolution at the time of projection becomes finer, so that the accuracy of calculation increases, but the calculation time becomes longer accordingly. This trade-off determines the image size that is actually used.
実際の投影画像の作成は以下のようにして行う。一例として、３次元座標データの一つ、点Ａ（ｘ，ｙ，ｚ）＝（５００ｍｍ，−３００ｍｍ，２０００ｍｍ）というデータを、ｙｚ平面に投影する場合について、説明する。 The actual projection image is created as follows. As an example, a case where data of one of the three-dimensional coordinate data, that is, point A (x, y, z) = (500 mm, −300 mm, 2000 mm) is projected on the yz plane will be described.
最初に、ｙｚ平面に投影された投影画像に含まれる全ての点の画素値を「０」で初期化しておく。 First, the pixel values of all points included in the projection image projected on the yz plane are initialized with “0”.
次に、点Ａのｙ座標、ｚ座標を抽出すると、（ｙ，ｚ）＝（−３００ｍｍ，２０００ｍｍ）となる。これを上記のｙｚ平面に投影するために、上記の縮尺と投影範囲を考慮して座標を換算すると、
縦（ｙ軸）：（−３００＋１２００）／１０＝９０
横（ｚ軸）：（２０００＋０）／１０＝２００
となるので、投影画像上の（ｙ，ｚ）＝（９０、２００）の点に対応することが分かる。従って、ｙｚ平面投影画像の（ｙ，ｚ）＝（９０，２００）の点の画素値を「０」から「１」に変更する。
Next, when the y coordinate and the z coordinate of the point A are extracted, (y, z) = (− 300 mm, 2000 mm). In order to project this onto the yz plane, the coordinates are converted in consideration of the scale and the projection range.
Vertical (y-axis): (−300 + 1200) / 10 = 90
Horizontal (z axis): (2000 + 0) / 10 = 200
Therefore, it can be seen that it corresponds to a point of (y, z) = (90, 200) on the projection image. Accordingly, the pixel value of the point (y, z) = (90, 200) in the yz plane projection image is changed from “0” to “1”.
次に、別のデータとして、点Ｂ（ｘ，ｙ，ｚ）＝（−４００ｍｍ，９００ｍｍ，１５００ｍｍ）が存在したとする。このデータを同様にｙｚ平面に投影すると、（ｙ，ｚ）＝（２１０、１５０）の点に相当するので、この点の画素値を同様に「０」から「１」に変更する。 Next, it is assumed that point B (x, y, z) = (− 400 mm, 900 mm, 1500 mm) exists as another data. Similarly, when this data is projected onto the yz plane, it corresponds to a point of (y, z) = (210, 150). Therefore, the pixel value at this point is similarly changed from “0” to “1”.
さらに、別のデータとして、点Ｃ（ｘ，ｙ，ｚ）＝（−２００ｍｍ，−３０３ｍｍ，１９９８ｍｍ）が存在したとする。このデータを同様にｙｚ平面に投影すると、（ｙ，ｚ）＝（９０、２００）の点に相当する。これは点Ａの投影後の座標と同じであり、既にこの点の画素値は「１」になっているため、ここでは何も行わない。これは、３次元座標上の異なる２点ＡとＣとが、ｙｚ平面への投影後は同一の点に投影されることを意味する。 Furthermore, it is assumed that the point C (x, y, z) = (− 200 mm, −303 mm, 1998 mm) exists as another data. When this data is similarly projected onto the yz plane, it corresponds to a point of (y, z) = (90, 200). This is the same as the projected coordinate of point A, and the pixel value of this point is already “1”, so nothing is done here. This means that two different points A and C on the three-dimensional coordinates are projected onto the same point after projection onto the yz plane.
以下同様にして、３次元座標データに含まれる点の全てについて、ｙ座標値・ｚ座標値の抽出、ｙｚ平面の座標値への換算を行い、該当する座標の画素値の「１」への変更を行う。これにより、３次元座標データに含まれる点が存在する部分のみ「１」となった２値画像の形でｙｚ平面投影画像が得られることになる。 In the same manner, y-coordinate values and z-coordinate values are extracted and converted into coordinate values on the yz plane for all points included in the three-dimensional coordinate data, and the pixel values of the corresponding coordinates are converted to “1”. Make a change. As a result, a yz plane projection image is obtained in the form of a binary image in which only the portion where the point included in the three-dimensional coordinate data exists is “1”.
・直線検出部３３
直線検出部３３は、投影画像生成部３２が生成した投影画像から、直線を検出する。例えば、図７に示す投影画像において、床面を示す直線を検出する場合について説明する。
・
The straight
図７は、図６の３次元座標データをｙｚ平面に投影した投影画像である。図７に示すように、ｙｚ平面への投影画像は、全ての画素について、ｙ座標とｚ座標とを抽出している。また、床面よりも下に物体が存在しない場合、同一のｚ座標を有する点同士を比較すると、床面を表す点が最も低い、即ちｙ座標が最も小さい点（以下、ボトム点と称する）であることが分かる。さらに、床面を表す平面が、投影画像では直線になっていることが分かる。 FIG. 7 is a projection image obtained by projecting the three-dimensional coordinate data of FIG. 6 onto the yz plane. As shown in FIG. 7, the y-coordinate and the z-coordinate are extracted from all the pixels in the projection image onto the yz plane. Further, when no object exists below the floor surface, when the points having the same z coordinate are compared, the point representing the floor surface is the lowest, that is, the y coordinate is the smallest (hereinafter referred to as the bottom point). It turns out that it is. Furthermore, it can be seen that the plane representing the floor is a straight line in the projected image.
前述したように、距離画像センサ２０は、ｚ軸を中心として回転する向きに傾いて取り付けられていないため、床面と距離画像センサ２０との傾き角は、ｘ軸を中心として回転する方向の傾きθに比べて十分小さい。その結果、床面を表す３次元座標データをｙｚ平面へ投影すると、ほぼ一つの直線上に分布する。
As described above, since the
ボトム点が床面を示すので、直線検出部３３は、投影画像に含まれる各画素列を、下から上方向にスキャンして、スキャン方向に沿って最初に見つかった点、即ち最初に「１」となった点のみを残し、他の点を削除する演算を行い、ボトム画像を得る。
Since the bottom point indicates the floor surface, the straight
次に、直線検出部３３は、得たボトム画像に対し、直線を検出するフィッティング処理を施し、直線の傾き、および切片等のパラメータを得る。直線検出の結果によっては直線の候補が一本ではなく複数本得られる場合があるが、その場合は予め定められた基準で最も確からしい直線を選出する。
Next, the straight
直線検出手法（フィッティング処理）には、Ｈｏｕｇｈ変換、その改良手法である確率的Ｈｏｕｇｈ変換、単純な最小自乗法、およびＲＡＮＳＡＣ法など任意の処理を適用できる。なお、前記「定められた基準」としては、例えば、最小自乗法やＲＡＮＳＡＣ法であれば、直線フィッティングを行った際の誤差（残差）が最も小さい直線を、最も確からしい直線として選択することができる。また、Ｈｏｕｇｈ変換であれば、その直線を支持する点の数が最も多いものを、最も確からしい直線として選択することができる。これらの処理により、直線の候補が複数存在する場合に、それぞれの手法で最も確からしい直線を抽出することができる。 Arbitrary processing such as Hough transform, stochastic Hough transform, which is an improved method thereof, simple least square method, and RANSAC method can be applied to the straight line detection method (fitting process). For example, in the case of the least square method or the RANSAC method, the straight line having the smallest error (residual) when performing straight line fitting is selected as the most probable straight line. Can do. In the case of the Hough transform, a line having the largest number of points that support the straight line can be selected as the most likely straight line. By these processes, when there are a plurality of straight line candidates, the most likely straight line can be extracted by each method.
以上により、ｙｚ平面投影画像上で床面を表す直線の情報を得ることができる。 As described above, it is possible to obtain straight line information representing the floor surface on the yz plane projection image.
・平面検出部３４
平面検出部３４は、直線検出部３３が検出した直線から、３次元座標データ内の平面を検出し、平面の高さ、および角度を算出する。検出された平面は、高さ、および角度という幾何的な情報以外に、例えば平面の方程式：ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０といった他の形式によって表現したパラメータを求めても構わない。
-
The
図６、および図７において平面として検出される床面は、距離画像センサ２０の取り付けられた高さ、および角度から、平面検出部３４によって検出される平面の高さ、および角度が推測可能である。したがって、検出される高さ、および角度の許容範囲を予め設定し、平面検出部３４が検出した平面が許容範囲内にあるかを判定することによって、より確実に床面を検出可能となる。
6 and 7, the height and angle of the plane detected by the
また、平面検出部３４は、検出した床面を、床平面情報として保有する。床平面情報は、平面検出部３４が床面を検出すると、随時更新される。こうすることにより、清掃ロボット１の移動に伴って発生する床平面の変動に追随し、常に床平面を把握することが出来る。また、距離画像センサの前を人が横切るなどして床面が一時的に検出できなくなっても、以前に検出した床平面情報を使用することで、床平面検出処理の欠落を防止することができる。
The
・障害物・段差検出部３５
障害物・段差検出部３５は、ｘｙｚ座標系の３次元座標データをＸＹＺ座標系の３次元座標データに変換する。そして、ＸＹＺ座標系において、各点と平面との距離を計算し、検出した平面より高いか低いかを判定する。
・ Obstacle /
The obstacle /
・データ統合部３６
データ統合部３６は、複数の距離画像から検出された複数の障害物、および複数の段差を、一つの障害物・段差データとして統合する。本実施形態においては、距離画像センサ２０ａ、および距離画像センサ２０ｂのそれぞれから取得した距離画像から障害物、および段差が検出されるので、それらの障害物、および段差の情報を一つに統合し、障害物・段差データを作成する。その際、例えば、遠距離用の距離画像センサ２０ｂで検出された障害物Ａの前に、近距離用の距離画像センサ２０ａで検出された別の障害物Ｂがある場合、距離の近い障害物Ｂのデータが優先するようにしてデータを統合することもできる。
The
障害物・段差データの形式は、後で走行制御部４１が処理しやすいように、任意の形式に変換することができる。データの座標系は、清掃ロボット基準のＸＹＺ座標系のまま出力することもでき、また極座標系（Ｒ−θ座標系）に変換することもできる。検出した全ての障害物、および段差をデータ化する手法以外に、データの間引き、または補間を行ったり、清掃ロボット１本体に最も近い障害物、および段差データのみを抽出したりする手法も考えられる。 The format of the obstacle / step data can be converted into an arbitrary format so that the traveling control unit 41 can easily process later. The coordinate system of the data can be output as the cleaning robot reference XYZ coordinate system, or can be converted into a polar coordinate system (R-θ coordinate system). In addition to the method of converting all detected obstacles and steps into data, a method of thinning out or interpolating data or extracting only the obstacles and step data closest to the main body of the cleaning robot 1 can be considered. .
（演算装置３０の処理）
以下に、演算装置３０の処理フローを纏める。図８は、本発明の実施形態１に係る清掃ロボット１の演算装置３０が処理する手順を示すフローチャートである。
(Processing of the arithmetic unit 30)
The processing flow of the
まず、３次元座標演算部３１が、距離画像センサ２０が生成した距離画像を取得する（ステップＳ１０１）。本実施形態１においては、近距離用の距離画像センサ２０ａと、遠距離用の距離画像センサ２０ｂとの２つの距離画像センサ２０が取り付けられているので、近距離用の距離画像と、遠距離用の距離画像とを、それぞれの距離画像センサ２０から取得する。
First, the three-dimensional coordinate
次に、３次元座標演算部３１が、取得した距離画像を、ｘｙｚ座標系の３次元座標データに変換する（ステップＳ１０２）。変換された３次元座標データから、投影画像生成部３２はｙｚ平面に投影された投影画像を生成する（ステップＳ１０３）。前述したように、本実施形態１においては、距離画像センサ２０は、ｚ軸を中心として回転する向きに傾いて取り付けられていないため、床面と距離画像センサ２０とのｚ軸を中心として回転する方向の傾きは、ｘ軸を中心として回転する方向の傾きに比べて十分に小さい。したがって、３次元座標データにおいて床面を表す平面は、ｙｚ平面への投影画像においては一直線上の点群となる。ここで、実際の投影画像を、図９に示す。
Next, the three-dimensional coordinate
図９中の（ａ）は、本発明の実施形態１に係る遠距離用の距離画像センサ２０ｂにより撮影された距離画像から生成されたｙｚ平面への投影画像であり、図９中の（ｂ）は、本発明の実施形態１に係る図９中の（ａ）のボトム画像であり、図９中の（ｃ）は、本発明の実施形態１に係る近距離用の距離画像センサ２０ａにより撮影された距離画像から生成されたｙｚ平面への投影画像であり、図９中の（ｄ）は、本発明の実施形態１に係る図９中の（ｃ）のボトム画像である。図９中の（ａ）および（ｃ）において、床面を表す点群６１、および点群６２は、前述したように、直線になっていることが分かる。
(A) in FIG. 9 is a projection image onto the yz plane generated from the distance image photographed by the
続いて、直線検出部３３が、投影画像からボトム画像を生成する（ステップＳ１０４）。図９（ｂ）、および（ｄ）に示すように、ボトム画像の直線は、床面を表す点群６１、および床面を表す点群６２と一致している。このボトム画像から、直線検出部３３が、直線を検出する（ステップＳ１０５）。そして、平面検出部３４が、検出された直線から、３次元座標データ内の平面を検出し、当該平面の角度および高さを算出する（ステップＳ１０６）。ここで平面の角度とは、ｚ軸に対する角度（傾斜角）である。また、平面の高さとは、床面と距離画像センサ２０との間の離間距離である。
Subsequently, the straight
算出された平面の角度、および高さは、平面検出部３４により、予め設定された角度、および高さの許容範囲内にあるか判定される（ステップＳ１０７）。
The calculated plane angle and height are determined by the
ステップＳ１０７において、「角度、および高さが許容範囲内である」と判定された場合（ステップＳ１０７：Ｙｅｓ）、検出された平面は床面なので、平面検出部３４は床平面情報を更新する（ステップＳ１０８）。一方、ステップＳ１０７において、「角度、および高さが許容範囲内ではない」と判定された場合（ステップＳ１０７：Ｎｏ）、検出された平面は床面ではないので、床平面情報は更新しない（ステップＳ１０９）。
If it is determined in step S107 that “the angle and the height are within the allowable range” (step S107: Yes), since the detected plane is the floor surface, the
ステップＳ１０８もしくはステップＳ１０９の後、障害物・段差検出部３５が、ｘｙｚ座標系からＸＹＺ座標系へと、３次元座標データを変換する（ステップＳ１１０）。次に、障害物・段差検出部３５が、変換したＸＹＺ座標系の３次元座標データから、各点と平面との距離を計算し、検出した平面より高いか低いかを判定し、障害物および段差を検出する（ステップＳ１１１）。当該判定には、閾値ｔを設け、床平面との距離がｔよりも大きければ床よりも高い障害物、もしくは段差、−ｔよりも小さければ床平面より低い段差と判定する。閾値ｔは、床平面の凹凸の大きさや、距離画像センサの計測誤差等を考慮して予め設定する。これにより、３次元座標データに含まれる全ての点について、その点が段差に属するか、障害物に属するか、あるいはそれ以外であるかが判定される。そして、各点の座標（Ｘ，Ｙ，Ｚ）に、その点が段差に属するか、障害物に属するか、それ以外であるかを表す情報Ｆが付加され、（Ｘ，Ｙ，Ｚ，Ｆ）形式に変換される。このようにして得られた段差および障害物に関する情報が、障害物・段差検出部３５からデータ統合部３６に渡される。
After step S108 or step S109, the obstacle /
そして、データ統合部３６が、障害物・段差検出部３５が検出した障害物および段差の情報を統合し、障害物・段差データを作成する（ステップＳ１１２）。最後に、データ統合部３６は、障害物・段差データを走行制御部４１に出力する（ステップＳ１１３）。
Then, the
このような手順により、演算装置３０は、距離画像から障害物・段差データを高速かつより確実に作成し、走行制御部４１に出力する。従って、清掃ロボット１は、障害物、および段差を回避しながら移動することが可能となる。また、複数の距離センサから独立に平面検出を行い、それらのデータを統合することで、さらに広い範囲の障害物、および段差を検出し、それらを回避しながら移動することができる。
By such a procedure, the
また、一般的に３次元空間上において、一つの平面を特定するには、３つのパラメータを特定する必要がある。しかし、本実施形態においては、距離画像センサ２０が床面に対して、ｚ軸を中心として回転する向きに傾いて取り付けられていないという条件を利用し、高さ方向（ｙ軸方向）と奥行き方向（ｚ軸方向）の傾きのみを求めている。即ち、特定するパラメータの数を２つに限定しているため、３つのパラメータを特定する場合に比べて高速な平面検出が可能となり、自律移動装置においても容易にリアルタイムの床面検出が実現できる。
In general, in order to specify one plane in a three-dimensional space, it is necessary to specify three parameters. However, in the present embodiment, the height direction (y-axis direction) and the depth are utilized by using the condition that the
（段差の検出）
前述した実施形態１においては、床面よりも下に物体が存在しない場合が望ましいが、例えば、床面よりも低い面の段差が存在する場合がある。このような場合における床面の検出方法について、以下に説明する。
(Step detection)
In the first embodiment described above, it is desirable that no object exists below the floor surface, but for example, there may be a level difference on the surface lower than the floor surface. A method for detecting the floor surface in such a case will be described below.
図１０中の（ａ）は、床面より低い段差が存在する場合のｙｚ平面への投影画像であり、図１０中の（ｂ）は、図１０中の（ａ）のボトム画像である。図１０中の（ａ）に示すように、ｙｚ平面への投影画像において、床面を表す点群６１と、段差を表す点群６２とが直線になっている。このような場合は、図１０中の（ｂ）に示すように、距離画像センサ２０の取り付けられた高さ、角度から、予め床面が存在すると予想される範囲を限定し、それを直線検出の際の許容範囲６４として設定する。許容範囲６４を設定することにより、直線検出部３３は、段差を表す点群６３ではなく、床面を表す点群６１を直線として検出可能となる。
(A) in FIG. 10 is a projection image onto the yz plane when there is a step difference lower than the floor surface, and (b) in FIG. 10 is a bottom image in (a) in FIG. As shown to (a) in FIG. 10, in the projection image to yz plane, the
例えば、遠距離用の距離画像センサ２０ｂが、高さＨ＝７１０［ｍｍ］、角度θ＝２２．５［ｄｅｇ］に取り付けられているとする。このとき、理論上の床面は、距離画像センサ基準の座標系で、原点から７１０［ｍｍ］の距離に、ｚｘ平面とのなす角（奥行き方向の傾き角）２２．５［ｄｅｇ］で存在することになる。しかし、取り付け位置は組み立て誤差等により多少変動するため、ステップＳ１０６において、算出された床面の高さ、および角度が、前記の値から±数ｍｍ、±数ｄｅｇの範囲内にあるかどうかを確認する。確認の結果、範囲内にあれば床面であるとしてこの直線を検出し、範囲内になければ、ステップＳ１０５において検出された複数の直線から別の直線を選択し、同様に範囲内にあるかどうかを確認する。
For example, it is assumed that the
このように、検出された平面の高さ、および角度を限定して床面を検出すれば、距離画像内に床面より低い段差が含まれていても、それを除外し、床面を検出することができる。 In this way, if the floor surface is detected by limiting the height and angle of the detected plane, even if the distance image includes a level difference lower than the floor surface, it is excluded and the floor surface is detected. can do.
〔実施形態２〕
本発明の第２の実施形態について、図１１〜図１２を用いて説明する。なお、説明の便宜上、前記実施形態にて説明した部材と同じ機能を有する部材については、同じ符号を付記し、その説明を省略する。
[Embodiment 2]
A second embodiment of the present invention will be described with reference to FIGS. For convenience of explanation, members having the same functions as those described in the embodiment are given the same reference numerals, and descriptions thereof are omitted.
実施形態１においては、床面と距離画像センサ２０との傾きのうち、ｚ軸を中心として回転する方向の傾きが、ｘ軸を中心として回転する方向の傾きに比べて十分小さく無視できる場合について説明した。これに対して本実施形態２では、ｚ軸を中心として回転する方向の傾きがｘ軸を中心として回転する方向の傾きに比べて小さいものの、完全には無視できない場合における平面検出について、以下に説明する。
In the first embodiment, among the inclinations of the floor surface and the
（演算装置３０の処理）
図１１は、本実施形態２に係る清掃ロボットに具備される演算装置３０が処理する手順を示すフローチャートである。なお、図８のフローチャートと同じステップについては、説明を省略する。また、図１２中の（ａ）は、本実施形態２に係る３次元座標データの例を示す図であり、図１２中の（ｂ）は、図１２中の（ａ）の３次元座標データをｙｚ平面に投影した投影画像であり、図１２中の（ｃ）は、図１２中の（ａ）の３次元座標データをｘｙ平面に投影した投影画像であり、図１２中の（ｄ）は、図１２中の（ａ）の３次元座標データをｘｙ´平面に投影した投影画像である。
(Processing of the arithmetic unit 30)
FIG. 11 is a flowchart illustrating a procedure performed by the
図１１において、ステップＳ１０６の処理である、検出した平面のｘ軸を中心として回転する向きの角度θ、および高さを算出する。しかし、図１２中の（ｂ）に示すように、ｙｚ平面への投影画像で得られる点群は完全に一直線上には並ばず、幅を有して帯状に分布している。 In FIG. 11, the angle θ and the height of the rotation direction about the x-axis of the detected plane, which is the process of step S <b> 106, are calculated. However, as shown in (b) of FIG. 12, the point cloud obtained by the projection image on the yz plane is not completely aligned on a straight line but is distributed in a band shape with a width.
そこで、ｘｙｚ座標系を、ｘ軸を中心として回転する方向の傾きθに相当する角度だけ回転させたｘｙ´ｚ´座標を、新たに定義する。例えば、床面がｘｙｚ座標系で原点から７１０［ｍｍ］の距離に、ｚｘ平面とのなす角（奥行き方向の傾き角）２２．５［ｄｅｇ］で存在する場合、ｙ´軸およびｚ´軸は、ｙ軸およびｚ軸をそれぞれｘ軸を中心に２２．５［ｄｅｇ］回転させて得られる。このように、３次元座標演算部３１は、ｘｙｚ座標系における３次元座標データを、新たに定義したｘｙ´ｚ´座標系に変換する（ステップＳ１２１）。
Therefore, xy′z ′ coordinates obtained by rotating the xyz coordinate system by an angle corresponding to the inclination θ in the direction of rotation about the x axis are newly defined. For example, when the floor surface is present at an angle (inclination angle in the depth direction) 22.5 [deg] with the zx plane at a distance of 710 [mm] from the origin in the xyz coordinate system, the y ′ axis and the z ′ axis Is obtained by rotating the y-axis and the z-axis by 22.5 [deg] around the x-axis, respectively. In this way, the three-dimensional coordinate
次に、投影画像生成部３２は、変換された３次元座標データをｘｙ´平面に投影し、投影画像を生成する（ステップＳ１２２）。その後、直線検出部３３は、投影画像のボトム画像を生成し（ステップＳ１２３）、ボトム画像から直線を検出する（ステップＳ１２４）。直線検出部３３が検出した直線から、平面検出部３４は左右方向の角度を得ることができ（ステップＳ１２５）、ステップＳ１０６で得たｘ軸を中心として回転する方向の傾きθと合わせて、角度、および高さが設定範囲内であるか判定する（ステップＳ１０７）。以下、実施形態１と同様である。
Next, the projection
このような手順により、演算装置３０は、ｚ軸を中心に回転する方向への傾きが無視できない場合においても、第１段階としてｘｙｚ座標系においてｘ軸を中心として回転する方向の傾きθを算出する。次に座標系をｘｙ´ｚ´に変換し、ｘｙ´平面への投影画像から、第２段階として左右方向の角度を算出する。このような２段階の処理により、より確実に床面を検出することができる。
By such a procedure, the
なお、ステップＳ１２１のような座標変換を行わず、投影画像生成部３２が元のｘｙｚ座標系のままｘｙ平面への投影画像を生成すると、図１２中の（ｃ）のような投影画像を得る。図１２中の（ｃ）に示すように、この画像では床面を表す点群が一直線上に並んでいない。したがって、例えば、図１２中の（ｃ）のＡ−Ａ´のラインに別の物体が存在し、床面が見えない状態にある場合、ボトム点の集合から直線を抽出したときに、Ａ−Ａ´ラインが床面を表す直線だと誤検出してしまうことになる。一方、最初にｘ軸を中心として回転する方向の傾きθだけｘ軸を中心に回転させたｘｙ´ｚ´座標系においては、ｚ´軸が床面とほぼ平行になっているため、ｘｙ´平面への投影画像は図１２中の（ｄ）に示すように、床面を表す点群がボトム点となり、抽出できる。
If the projection
〔まとめ〕
本発明の一態様に係る平面検出装置は、特定の検出対象平面を含む被写体の距離画像データから当該特定の検出対象平面を検出する平面検出装置（平面検出装置６０）であって、前記距離画像データを、前記特定の検出対象平面を表す検出対象３次元点群を含む３次元座標データに変換する３次元座標演算手段（３次元座標演算部３１）と、前記３次元座標データを所定の２次元平面に投影して、前記検出対象３次元点群が線形に分布した投影画像データを生成する投影画像生成手段（投影画像生成部３２）と、前記投影画像データから前記線形の直線を検出する直線検出手段（直線検出部３３）と、前記直線検出手段の検出結果に基づいて、前記特定の検出対象平面の傾きに関する情報を含む平面パラメータを算出する平面パラメータ算出手段（平面検出部３４）と、を備えることを特徴としている。
[Summary]
A flat surface detection device according to an aspect of the present invention is a flat surface detection device (a flat surface detection device 60) that detects a specific detection target plane from distance image data of a subject including a specific detection target plane, and the distance image Three-dimensional coordinate calculation means (three-dimensional coordinate calculation unit 31) for converting data into three-dimensional coordinate data including a detection target three-dimensional point group representing the specific detection target plane; Projection image generation means (projection image generation unit 32) for generating projection image data in which the three-dimensional point cloud to be detected is linearly distributed by projecting onto a three-dimensional plane, and detecting the linear straight line from the projection image data A plane parameter calculation for calculating a plane parameter including information on the inclination of the specific detection target plane based on a detection result of the line detection unit (line detection unit 33) and the line detection unit. Stage and (planar detector unit 34) is characterized in that it comprises.
前記の構成によれば、平面を含む距離画像データを３次元座標データに変換し、変換した３次元座標データを平面に投影する。そして、投影した画像から直線を検出し、その直線に基づいた平面パラメータを検出する。したがって、距離画像の中に平面検出の手掛かりを必要とせず、また障害物等の平面とは無関係の情報が多数含まれていても、平面をより確実に検出できるという効果を奏する。 According to the above configuration, distance image data including a plane is converted into three-dimensional coordinate data, and the converted three-dimensional coordinate data is projected onto the plane. Then, a straight line is detected from the projected image, and a plane parameter based on the straight line is detected. Therefore, the range image does not require a clue for plane detection, and even if a lot of information unrelated to the plane such as an obstacle is included, the plane can be detected more reliably.
さらに、本発明の一態様に係る平面検出装置（平面検出装置６０）は、前記距離画像データにおいて、前記被写体の奥行き方向をｚ軸として、当該ｚ軸に対して垂直であるｘ軸およびｙ軸であって、当該距離画像データの左右方向をｘ軸、上下方向をｙ軸としたｘｙｚ座標系において、前記投影画像生成手段（投影画像生成部３２）は、前記３次元座標データをｙｚ平面に投影した投影画像データを生成し、前記平面パラメータ算出手段（平面検出部３４）は、投影画像データから、前記特定の検出対象平面の、前記ｚ軸に対する傾斜角を含む前記平面パラメータを算出することを特徴としている。 Furthermore, the flat surface detection device (flat surface detection device 60) according to one aspect of the present invention uses the depth direction of the subject as the z axis in the distance image data, and the x axis and the y axis perpendicular to the z axis. In the xyz coordinate system in which the horizontal direction of the distance image data is the x axis and the vertical direction is the y axis, the projection image generation means (projection image generation unit 32) sets the three-dimensional coordinate data on the yz plane. Projected projection image data is generated, and the plane parameter calculation means (plane detection unit 34) calculates the plane parameter including an inclination angle of the specific detection target plane with respect to the z axis from the projection image data. It is characterized by.
前記の構成によれば、ｙｚ平面に投影した投影画像データから平面を検出できることにより、より高速に平面を検出できるという効果を奏する。 According to the above configuration, since the plane can be detected from the projection image data projected onto the yz plane, there is an effect that the plane can be detected at a higher speed.
さらに、本発明の一態様に係る前記平面パラメータ算出手段は、前記直線が予め定めた範囲内にあるか否かを判別することを特徴としている。 Furthermore, the plane parameter calculation means according to an aspect of the present invention is characterized by determining whether or not the straight line is within a predetermined range.
前記の構成によれば、予め検出する直線の範囲を設定することによって、より確実に対象の平面を検出できるという効果を奏する。 According to the above configuration, there is an effect that the plane of the target can be detected more reliably by setting the range of the straight line to be detected in advance.
さらに、本発明の一態様に係る平面検出装置（平面検出装置６０）は、前記ｘｙｚ座標系を、前記ｘ軸を回転軸として回転させることによってｘｙ´ｚ´座標系に変換して、ｘｙ´ｚ´座標系における前記特定の検出対象平面を表す検出対象３次元点群を含む第２の３次元座標データを生成する第２の３次元座標演算手段（３次元座標演算部３１）と、前記第２の３次元座標データを、ｘｙ´平面に投影して、当該第２の３次元座標データに含まれる前記検出対象３次元点群が線形に分布した第２の投影画像データを生成する第２の投影画像生成手段（投影画像生成部３２）と、前記第２の投影画像データから、当該第２の投影画像データに分布する前記線形の第２の直線を検出する第２の直線検出手段と（直線検出部３３）、前記第２の直線検出手段の検出結果に基づいて、前記特定の検出対象平面の傾きに関する情報を含む第２の平面パラメータを算出する第２の平面パラメータ算出手段（平面検出部３４）と、をさらに備えることを特徴としている。 Furthermore, the flat surface detection apparatus (the flat surface detection apparatus 60) according to an aspect of the present invention converts the xyz coordinate system into an xy′z ′ coordinate system by rotating the x axis about the rotation axis, and xy ′ second 3D coordinate calculation means (3D coordinate calculation unit 31) for generating second 3D coordinate data including a detection target 3D point group representing the specific detection target plane in the z ′ coordinate system; The second three-dimensional coordinate data is projected onto the xy ′ plane to generate second projection image data in which the detection target three-dimensional point group included in the second three-dimensional coordinate data is linearly distributed. Two projection image generation means (projection image generation unit 32) and second straight line detection means for detecting the linear second straight line distributed in the second projection image data from the second projection image data. (Straight line detection unit 33), the second straight line detection 2nd plane parameter calculation means (plane detection part 34) which calculates the 2nd plane parameter containing the information about the inclination of the specific detection object plane based on the detection result of the output means, It is said.
前記の構成によれば、より確実に対象の平面を検出できるという効果を奏する。 According to the above configuration, there is an effect that the target plane can be detected more reliably.
さらに、本発明の一態様に係る自律移動装置（清掃ロボット１）は、平面検出装置（平面検出装置６０）と、前記距離画像データを生成する距離画像生成手段（距離画像センサ２０）と、走行手段（駆動輪２）と、を備えており、前記平面検出装置を用いて走行路となる平面を検出することを特徴とする自律移動装置。 Furthermore, an autonomous mobile device (cleaning robot 1) according to one aspect of the present invention includes a flat surface detection device (flat surface detection device 60), distance image generation means (distance image sensor 20) that generates the distance image data, and travel. Means (driving wheel 2), and an autonomous mobile device that detects a plane as a traveling path using the plane detection device.
前記の構成によれば、前記自律移動装置は、前記平面検出装置と同様の効果を奏することができる。 According to the said structure, the said autonomous mobile apparatus can have an effect similar to the said plane detection apparatus.
さらに、本発明の一態様に係る自律移動装置（清掃ロボット１）は、前記距離画像生成手段（距離画像センサ２０）を複数備え、各距離画像生成手段が生成する距離画像データそれぞれから前記平面パラメータを算出することを特徴としている。 Furthermore, the autonomous mobile device (cleaning robot 1) according to an aspect of the present invention includes a plurality of the distance image generating means (distance image sensor 20), and the plane parameter is calculated from each of the distance image data generated by each distance image generating means. It is characterized by calculating.
前記の構成によれば、より広範囲においてより確実に平面を検出できるという効果を奏する。 According to the above configuration, there is an effect that the plane can be detected more reliably in a wider range.
〔補足説明〕
本発明は前述した各実施形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施形態についても本発明の技術的範囲に含まれる。さらに、各実施形態にそれぞれ開示された技術的手段を組み合わせることにより、新しい技術的特徴を形成することができる。
[Supplementary explanation]
The present invention is not limited to the above-described embodiments, and various modifications are possible within the scope of the claims, and the embodiments can be obtained by appropriately combining technical means disclosed in different embodiments. Is also included in the technical scope of the present invention. Furthermore, a new technical feature can be formed by combining the technical means disclosed in each embodiment.
（距離画像センサ２０について）
前述した実施形態においては、距離画像センサ２０は赤外線投射方式を用いたが、ステレオ方式やＴＯＦ方式など、他方式の距離画像センサを用いることも可能である。ステレオ方式の場合、ステレオ配置された左右カメラから得られる左右画像に対し、対応点探索などの手法によって視差を計算する。視差の値から三角測量の原理によって対象物までの距離を求めることができる。このようにして得られた距離画像について、前述した実施形態と同様の処理により、平面検出を実現できる。
(About the distance image sensor 20)
In the above-described embodiment, the
（検出できる平面）
前述した実施形態においては、床面を検出したが、路面、水面、壁面、および天井面など、他の平面の検出にも用いることができる。
(Detectable plane)
In the embodiment described above, the floor surface is detected, but it can also be used to detect other planes such as a road surface, a water surface, a wall surface, and a ceiling surface.
（自律移動装置）
前述した実施形態においては、自律移動装置として清掃ロボット１について説明したが、例えば工場内の無人搬送機、自動走行車、介護用ロボット、防犯用ロボット、災害救助用ロボット、エンターテインメント用ロボットなど、他の自律移動装置にも適用可能である。
(Autonomous mobile device)
In the embodiment described above, the cleaning robot 1 has been described as an autonomous mobile device. It can also be applied to other autonomous mobile devices.
（平面検出装置）
前述した実施形態においては、平面検出装置６０を清掃ロボット１に組み込んで利用したが、平面検出装置６０を独立した装置として産業用、民生用、その他用途に用いる他、汎用的な携帯情報端末などの一部に組み込むことも可能である。
(Flat surface detector)
In the above-described embodiment, the flat
（ソフトウェアによる実現例）
最後に、平面検出装置の各ブロックは、集積回路（ＩＣチップ）上に形成された論理回路によってハードウェア的に実現してもよいし、ＣＰＵ（Central Processing Unit）を用いてソフトウェア的に実現してもよい。
(Example of software implementation)
Finally, each block of the flat panel detector may be realized in hardware by a logic circuit formed on an integrated circuit (IC chip) or in software using a CPU (Central Processing Unit). May be.
後者の場合、平面検出装置は、各機能を実現するプログラムの命令を実行するＣＰＵ、前記プログラムを格納したＲＯＭ（Read Only Memory）、前記プログラムを展開するＲＡＭ（Random Access Memory）、前記プログラムおよび各種データを格納するメモリ等の記憶装置（記録媒体）などを備えている。そして、本発明の目的は、前述した機能を実現するソフトウェアである平面検出装置の制御プログラムのプログラムコード（実行形式プログラム、中間コードプログラム、ソースプログラム）をコンピュータで読み取り可能に記録した記録媒体を、前記平面検出装置に供給し、そのコンピュータ（またはＣＰＵやＭＰＵ）が記録媒体に記録されているプログラムコードを読み出し実行することによっても、達成可能である。 In the latter case, the flat panel detector includes a CPU that executes instructions of a program that realizes each function, a ROM (Read Only Memory) that stores the program, a RAM (Random Access Memory) that expands the program, the program, and various types of programs. A storage device (recording medium) such as a memory for storing data is provided. An object of the present invention is to provide a recording medium in which a program code (execution format program, intermediate code program, source program) of a control program for a flat panel detector, which is software that realizes the functions described above, is recorded so as to be readable by a computer This can also be achieved by supplying the flat surface detection device and reading and executing the program code recorded on the recording medium by the computer (or CPU or MPU).
前記記録媒体としては、一時的でない有形の媒体（non-transitory tangible medium）、例えば、磁気テープやカセットテープ等のテープ類、フロッピー（登録商標）ディスク／ハードディスク等の磁気ディスクやＣＤ−ＲＯＭ／ＭＯ／ＭＤ／ＤＶＤ／ＣＤ−Ｒ等の光ディスクを含むディスク類、ＩＣカード（メモリカードを含む）／光カード等のカード類、マスクＲＯＭ／ＥＰＲＯＭ／ＥＥＰＲＯＭ（登録商標）／フラッシュＲＯＭ等の半導体メモリ類、あるいはＰＬＤ（Programmable logic device）やＦＰＧＡ（Field Programmable Gate Array）等の論理回路類などを用いることができる。 Examples of the recording medium include a non-transitory tangible medium such as a magnetic tape and a cassette tape, a magnetic disk such as a floppy (registered trademark) disk / hard disk, and a CD-ROM / MO. Discs including optical discs such as / MD / DVD / CD-R, cards such as IC cards (including memory cards) / optical cards, semiconductor memories such as mask ROM / EPROM / EEPROM (registered trademark) / flash ROM Alternatively, logic circuits such as PLD (Programmable Logic Device) and FPGA (Field Programmable Gate Array) can be used.
また、平面検出装置を通信ネットワークと接続可能に構成し、前記プログラムコードを通信ネットワークを介して供給してもよい。この通信ネットワークは、プログラムコードを伝送可能であればよく、特に限定されない。例えば、インターネット、イントラネット、エキストラネット、ＬＡＮ、ＩＳＤＮ、ＶＡＮ、ＣＡＴＶ通信網、仮想専用網（Virtual Private Network）、電話回線網、移動体通信網、衛星通信網等が利用可能である。また、この通信ネットワークを構成する伝送媒体も、プログラムコードを伝送可能な媒体であればよく、特定の構成または種類のものに限定されない。例えば、ＩＥＥＥ１３９４、ＵＳＢ、電力線搬送、ケーブルＴＶ回線、電話線、ＡＤＳＬ（Asymmetric Digital Subscriber Line）回線等の有線でも、ＩｒＤＡやリモコンのような赤外線、Ｂｌｕｅｔｏｏｔｈ（登録商標）、ＩＥＥＥ８０２．１１無線、ＨＤＲ（High Data Rate）、ＮＦＣ（Near Field Communication）、ＤＬＮＡ（Digital Living Network Alliance）、携帯電話網、衛星回線、地上波デジタル網等の無線でも利用可能である。なお、本発明は、前記プログラムコードが電子的な伝送で具現化された、搬送波に埋め込まれたコンピュータデータ信号の形態でも実現され得る。 Further, the flat detection device may be configured to be connectable to a communication network, and the program code may be supplied via the communication network. The communication network is not particularly limited as long as it can transmit the program code. For example, the Internet, intranet, extranet, LAN, ISDN, VAN, CATV communication network, virtual private network, telephone line network, mobile communication network, satellite communication network, and the like can be used. The transmission medium constituting the communication network may be any medium that can transmit the program code, and is not limited to a specific configuration or type. For example, even with wired lines such as IEEE 1394, USB, power line carrier, cable TV line, telephone line, and ADSL (Asymmetric Digital Subscriber Line) line, infrared rays such as IrDA and remote control, Bluetooth (registered trademark), IEEE 802.11 wireless, HDR ( It can also be used by radio such as High Data Rate (NFC), Near Field Communication (NFC), Digital Living Network Alliance (DLNA), mobile phone network, satellite line, and digital terrestrial network. The present invention can also be realized in the form of a computer data signal embedded in a carrier wave in which the program code is embodied by electronic transmission.
本発明は、計測対象となる画像データに含まれる床などの平面を検出するための平面検出装置およびそれを用いた自律移動装置に関するものであり、平面検出装置自体を独立した装置として産業用、民生用、その他用途に用いる他、他の装置の一部に組み込んで利用したり、装置の一部または全部を集積回路（ＩＣチップ）化したりして利用することができる。 The present invention relates to a plane detection device for detecting a plane such as a floor included in image data to be measured and an autonomous mobile device using the same, and the plane detection device itself is an independent device for industrial use. In addition to being used for consumer use and other purposes, it can be used by being incorporated into a part of another device, or part or all of the device can be used as an integrated circuit (IC chip).
１ 清掃ロボット（自律移動装置）
２ 駆動輪（走行手段）
３ 従輪
４ バッテリ
５ 洗浄液タンク
６ 洗浄液吐出部
７ 廃液タンク
８ 吸引口
９ 清掃ブラシ
１０ モータ
１１ 筐体
１２ 保護部材
２０ 距離画像センサ（距離画像生成手段）
２０ａ 近距離用の距離画像センサ
２０ｂ 遠距離用の距離画像センサ
３０ 演算装置
３１ ３次元座標演算部（３次元座標演算手段、第２の３次元座標演算手段）
３２ 投影画像生成部（投影画像生成手段、第２の投影画像生成手段）
３３ 直線検出部（直線検出手段、第２の直線検出手段）
３４ 平面検出部（平面パラメータ算出手段、第２の平面パラメータ算出手段）
３５ 障害物・段差検出部
３６ データ統合部
４１ 走行制御部
４２ 清掃制御部
４３ マップ情報メモリ部
４４ 状態表示部
４５ 廃液回収ユニット
４６ 洗浄液吐出ユニット
４７ ロータリーエンコーダ
４８ 駆動輪モータ
４９ ジャイロセンサ
５０ 制御スイッチ
６０ 平面検出装置
1 Cleaning robot (autonomous mobile device)
2 Drive wheels (traveling means)
3 Subordinate wheel 4
20a Distance image sensor for
32 Projection image generation unit (projection image generation means, second projection image generation means)
33 Straight line detection unit (straight line detection means, second straight line detection means)
34 Plane detector (plane parameter calculation means, second plane parameter calculation means)
35 Obstacle /
Claims (4)
前記距離画像データを、前記特定の検出対象平面を表す検出対象３次元点群を含む３次元座標データに変換する３次元座標演算手段と、
前記３次元座標データを所定の２次元平面に投影して、前記検出対象３次元点群が線形に分布した投影画像データを生成する投影画像生成手段と、
前記投影画像データから前記線形の直線を検出する直線検出手段と、
前記直線検出手段の検出結果に基づいて、前記特定の検出対象平面の傾きに関する情報を含む平面パラメータを算出する平面パラメータ算出手段と、
を備え、
前記距離画像データにおいて、前記被写体の奥行き方向をｚ軸として、当該ｚ軸に対して垂直であるｘ軸およびｙ軸であって、当該距離画像データの左右方向をｘ軸、上下方向をｙ軸としたｘｙｚ座標系において、前記投影画像生成手段は、前記３次元座標データをｙｚ平面に投影した投影画像データを生成し、
前記平面パラメータ算出手段は、投影画像データから、前記特定の検出対象平面の、前記ｚ軸に対する傾斜角を含む前記平面パラメータを算出することを特徴とする平面検出装置。 A plane detection device for detecting a specific detection target plane from distance image data of a subject including the specific detection target plane,
Three-dimensional coordinate calculation means for converting the distance image data into three-dimensional coordinate data including a detection target three-dimensional point group representing the specific detection target plane;
Projection image generating means for projecting the three-dimensional coordinate data onto a predetermined two-dimensional plane and generating projection image data in which the detection target three-dimensional point group is linearly distributed;
Straight line detecting means for detecting the linear straight line from the projection image data;
A plane parameter calculating means for calculating a plane parameter including information on the inclination of the specific detection target plane based on the detection result of the straight line detecting means;
With
In the distance image data, the depth direction of the subject is the z-axis, and the x-axis and the y-axis are perpendicular to the z-axis, the left-right direction of the distance image data is the x-axis, and the up-down direction is the y-axis In the xyz coordinate system, the projection image generation unit generates projection image data obtained by projecting the three-dimensional coordinate data onto the yz plane,
The plane parameter calculating means, the projection image data, the specific detection of the target plane, flat surface detector you and calculates the plane parameters including an inclination angle with respect to the z-axis.
前記第２の３次元座標データを、ｘｙ´平面に投影して、当該第２の３次元座標データに含まれる前記検出対象３次元点群が線形に分布した第２の投影画像データを生成する第２の投影画像生成手段と、
前記第２の投影画像データから、当該第２の投影画像データに分布する前記線形の第２の直線を検出する第２の直線検出手段と、
前記第２の直線検出手段の検出結果に基づいて、前記特定の検出対象平面の傾きに関する情報を含む第２の平面パラメータを算出する第２の平面パラメータ算出手段と、
をさらに備えることを特徴とする請求項１に記載の平面検出装置。 The xyz coordinate system is converted into an xy′z ′ coordinate system by rotating the x-axis as a rotation axis, and a detection target three-dimensional point group representing the specific detection target plane in the xy′z ′ coordinate system is converted. Second 3D coordinate calculation means for generating second 3D coordinate data including:
The second three-dimensional coordinate data is projected onto the xy ′ plane to generate second projection image data in which the detection target three-dimensional point group included in the second three-dimensional coordinate data is linearly distributed. Second projection image generation means;
Second line detection means for detecting the second linear line distributed in the second projection image data from the second projection image data;
Second plane parameter calculation means for calculating a second plane parameter including information on the inclination of the specific detection target plane based on the detection result of the second straight line detection means;
The flat panel detector according to claim 1 , further comprising:
前記距離画像データを生成する距離画像生成手段と、
走行手段と、を備えており、
前記平面検出装置を用いて走行路となる平面を検出することを特徴とする自律移動装置。 The flat surface detection device according to any one of claims 1 to 3 ,
Distance image generation means for generating the distance image data;
Traveling means, and
An autonomous mobile device that detects a plane as a traveling path using the plane detection device.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012235950A JP6030405B2 (en) | 2012-10-25 | 2012-10-25 | Planar detection device and autonomous mobile device including the same |
| PCT/JP2013/071855 WO2014064990A1 (en) | 2012-10-25 | 2013-08-13 | Plane detection device, autonomous locomotion device provided with plane detection device, method for detecting road level difference, device for detecting road level difference, and vehicle provided with device for detecting road level difference |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012235950A JP6030405B2 (en) | 2012-10-25 | 2012-10-25 | Planar detection device and autonomous mobile device including the same |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2014085940A JP2014085940A (en) | 2014-05-12 |
| JP6030405B2 true JP6030405B2 (en) | 2016-11-24 |
Family
ID=50788924
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2012235950A Active JP6030405B2 (en) | 2012-10-25 | 2012-10-25 | Planar detection device and autonomous mobile device including the same |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6030405B2 (en) |
Families Citing this family (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016065422A (en) * | 2014-09-26 | 2016-04-28 | 株式会社日立製作所 | Environment recognition device and excavator of using environment recognition device |
| KR101598385B1 (en) * | 2014-11-11 | 2016-03-02 | 한양대학교 산학협력단 | Autonomous driving method and robot using recognition scene based on straight line information |
| JP6781535B2 (en) * | 2015-07-27 | 2020-11-04 | シャープ株式会社 | Obstacle determination device and obstacle determination method |
| WO2017022079A1 (en) * | 2015-08-04 | 2017-02-09 | 日産自動車株式会社 | Step detection device and step detection method |
| JP7007078B2 (en) * | 2015-10-08 | 2022-01-24 | 東芝ライフスタイル株式会社 | Vacuum cleaner |
| JP6705636B2 (en) | 2015-10-14 | 2020-06-03 | 東芝ライフスタイル株式会社 | Vacuum cleaner |
| JP6649054B2 (en) * | 2015-11-19 | 2020-02-19 | アイシン精機株式会社 | Moving body |
| JP6668740B2 (en) * | 2015-12-22 | 2020-03-18 | いすゞ自動車株式会社 | Road surface estimation device |
| JP6654999B2 (en) * | 2016-12-12 | 2020-02-26 | 株式会社Ｓｏｋｅｎ | Target detection device |
| JP2018136896A (en) * | 2017-02-24 | 2018-08-30 | キヤノン株式会社 | Information processor, system, information processing method, and manufacturing method of article |
| KR102461938B1 (en) * | 2017-04-04 | 2022-10-31 | 엘지전자 주식회사 | Method of identifying obstacle on a driving surface and robot implementing thereof |
| JP2019188556A (en) * | 2018-04-26 | 2019-10-31 | オムロン株式会社 | Sensor controller, robot system, sensor control method, and program |
| KR102155095B1 (en) * | 2019-03-26 | 2020-09-11 | 엘지전자 주식회사 | Robot cleaner |
| CN111310663A (en) * | 2020-02-17 | 2020-06-19 | 北京三快在线科技有限公司 | Road fence detection method, device, equipment and storage medium |
| WO2023171611A1 (en) * | 2022-03-07 | 2023-09-14 | 興和株式会社 | Automatic travel robot |
| CN117731183B (en) * | 2024-02-19 | 2024-05-10 | 杭州万向职业技术学院 | Shield door cleaning robot and cleaning method thereof |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4344860B2 (en) * | 2004-01-30 | 2009-10-14 | 国立大学法人東京工業大学 | Road plan area and obstacle detection method using stereo image |
| JP5567908B2 (en) * | 2009-06-24 | 2014-08-06 | キヤノン株式会社 | Three-dimensional measuring apparatus, measuring method and program |
-
2012
- 2012-10-25 JP JP2012235950A patent/JP6030405B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2014085940A (en) | 2014-05-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6030405B2 (en) | Planar detection device and autonomous mobile device including the same | |
| WO2014064990A1 (en) | Plane detection device, autonomous locomotion device provided with plane detection device, method for detecting road level difference, device for detecting road level difference, and vehicle provided with device for detecting road level difference | |
| JP6132659B2 (en) | Ambient environment recognition device, autonomous mobile system using the same, and ambient environment recognition method | |
| JP6633568B2 (en) | Autonomous coverage robot | |
| CN106489104B (en) | System and method for use of optical odometry sensors in a mobile robot | |
| EP3104194B1 (en) | Robot positioning system | |
| EP3415281B1 (en) | Robot cleaner and method for controlling the same | |
| US8467902B2 (en) | Method and apparatus for estimating pose of mobile robot using particle filter | |
| US8679260B2 (en) | Methods and systems for movement of an automatic cleaning device using video signal | |
| KR20190131402A (en) | Moving Object and Hybrid Sensor with Camera and Lidar | |
| EP3447532A2 (en) | Hybrid sensor with camera and lidar, and moving object | |
| US8423225B2 (en) | Methods and systems for movement of robotic device using video signal | |
| KR20180080498A (en) | Robot for airport and method thereof | |
| KR102056147B1 (en) | Registration method of distance data and 3D scan data for autonomous vehicle and method thereof | |
| EP3716210B1 (en) | Three-dimensional point group data generation method, position estimation method, three-dimensional point group data generation device, and position estimation device | |
| CN111220148A (en) | Mobile robot positioning method, system and device and mobile robot | |
| JP2023083305A (en) | Cleaning map display device | |
| KR20200059666A (en) | Moving device and method of detecting object thereof | |
| US20210361132A1 (en) | Cleaner and method for controlling same | |
| JP2014106638A (en) | Moving device and control method | |
| US11960296B2 (en) | Method and apparatus for autonomous mobile device | |
| KR100784125B1 (en) | Method for extracting coordinates of landmark of mobile robot with a single camera | |
| KR20130135652A (en) | Mobile robot and method for controlling the same | |
| JP7345132B2 (en) | Autonomous vacuum cleaner, autonomous vacuum cleaner control method, and program | |
| AU2015224421B2 (en) | Autonomous coverage robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20151001 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20160823 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20160927 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20161018 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20161020 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6030405 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |