JPWO2005088244A1 - 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 - Google Patents
平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 Download PDFInfo
- Publication number
- JPWO2005088244A1 JPWO2005088244A1 JP2006511066A JP2006511066A JPWO2005088244A1 JP WO2005088244 A1 JPWO2005088244 A1 JP WO2005088244A1 JP 2006511066 A JP2006511066 A JP 2006511066A JP 2006511066 A JP2006511066 A JP 2006511066A JP WO2005088244 A1 JPWO2005088244 A1 JP WO2005088244A1
- Authority
- JP
- Japan
- Prior art keywords
- plane
- line segment
- distance data
- data point
- distance
- 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.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/024—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members specially adapted for moving on inclined or vertical surfaces
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/60—Type of objects
- G06V20/64—Three-dimensional objects
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
平面検出装置の平面検出部（３）は、画像を構成する距離データ点から同一平面にある距離データ点群を選択し、この距離データ点群から線分を抽出する線分抽出部（４）と、画像内に含まれる、線分抽出部（４）によって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部（５）とを有する。線分抽出部（４）は、まず、距離データ点群の端点を結ぶ線分Ｌ１を引き、この線分Ｌ１との距離が最も大きい着目点brkを探し、所定の値以上の場合は着目点でデータ点群を分割し、距離が所定の値未満の場合は最小二乗法により線分Ｌ２を求める。そして線分Ｌ２の一方の側にデータ点が所定の数以上連続して存在する場合、ジグザグ形と判断し、着目点brkでデータ点群を分割して、再び以上の処理を繰り返す。これによって、計測ノイズを含む距離データからノイズに対してロバストにかつ複数平面を同時に正確に検出する。
Description
本発明は、３次元距離データから平面を検出する平面検出装置、平面検出方法及び平面検出装置を搭載したロボット装置に関し、特に、線分拡張法（scan line grouping）により平面を検出する平面検出装置、平面検出方法及びロボット装置に関する。
本出願は、日本国において２００４年３月１７日に出願された日本特許出願番号２００４−０７７２１５を基礎として優先権を主張するものであり、これらの出願は参照することにより、本出願に援用される。
本出願は、日本国において２００４年３月１７日に出願された日本特許出願番号２００４−０７７２１５を基礎として優先権を主張するものであり、これらの出願は参照することにより、本出願に援用される。
３次元の距離情報から平面を検出するアルゴリズムについては様々な研究が行われている。検出された平面は、例えば移動式のロボット装置の障害物回避、または階段昇降動作の用途とすることができる。
一般に、距離情報から平面を検出する手法は、以下の手順により構成されている。
１．３次元距離情報を取得する
２．複数の平面に分割する
３．各平面に関して、重心、平面方程式、境界等を計算する
４．２、３の処理を繰り返し行い、最適な分割方法を獲得する
例えば、図１Ａに示す床面に載置された階段を有する画像は、図１Ｂに示すように、４つの平面領域Ａ、Ｂ、Ｃ、Ｄに分割される。ここで、領域Ａは床部分、領域Ｂ、Ｃ、Ｄは階段部分を示す。
次に、３次元の距離情報から平面を検出する従来の手法を下記文献の技術を例にとって説明する。例えば、Hooverら（A. Hoover， G. Jean-Baptiste， X. Jiang， P.J. Flynn， H. Bunke， D. Goldgof， K. Bowyer， D. Eggert， A. Fitzgibbon， and R. Fisher.），「距離画像セグメンテーションアルゴリズムの実験的比較（An experimental comparison of range image segmentation algorithms）」，Transaction on Pattern Analysis Machine Intelligence，18(7)，1996 では、距離情報から参照点をランダムにサンプルし、その点から空間的に近傍の点を連結することによって平面を検出する手法を提案している。また、Iocchiら（L. Iocchi， K. Konolige，and M. Bajracharya.），「ステレオ画像を使用した環境平面における現実の視覚地図（Visually realistic mapping of planar environment with stereo）」Int. Symposium on Experimental Robotics (ISER)，2000 では、距離情報からハフ変換によって統計的に平面を検出する手法を提案している。更に、Okadaら（K. Okada，S. Kagami，M. Inaba and H. Inoue.「平面セグメントの検索アルゴリズム，実装，アプリケーション（Plane Segment Finder: Algorithm，Implementation，and Applications）」Int. Conf. on Robotics and Automation (ICRA), 2001 では、距離情報からランダムに選んだ３点を平面方程式に当てはめ、統計的に平面を検出するランダマイズドハフ変換による手法を提案している。更にまた、Jiangら（X.-Y. Jiang and H. Bunke.）「線分拡張法によって距離画像を平面領域に高速に分割する手法（Fast segmentation of range images into planar regions by scan line grouping）」Machine Vision and Applications，7(2)，P115 122，1994 では、距離画像中の列に並んだ点群はそれらの点が３次元空間で同一平面上にあるならば、空間内で直線となることを利用し、線分拡張法(scan line grouping)による手法を提案している。
ところで、上述の各文献などのように、距離情報を使用した平面検出において重大な問題となるのが、ノイズに起因する、under-segmentation, over-segmentationの問題である。
under-segmentationとは、実際は複数の平面が存在するのにも拘わらず、ノイズの影響などによりそれらを例えば平均化したような１つの平面として認識してしまうことをいい、over-segmentationとは、実際は１つの同一平面であるのにも拘わらず、ノイズの影響などにより複数の異なる平面として認識してしまうものである。
図２に示すように、カメラ４０１Ｒ／Ｌにより取得された距離画像には、複数の踏面や、側面、床面などが含まれる。例えば図２の上図に示すように、例えばカメラ４０１Ｒ／Ｌの視野内に階段４００が存在する場合、距離画像内には複数の平面を含む。したがって、図２下図に示すように、ｘ−ｚ平面であれば、踏面４０２、側面４０３などの複数の平面を含むが、under-segmentationにより、これら複数の平面を区別できず、一つの平面４０３として検出してしまう。平面検出器は、一般に、平面の検出要求精度に比べてノイズの影響が大きい計測データから平面を検出するため、これを設計する際には、複数平面に分離する為の閾値を甘くしなければならず、従ってこのようなunder-segmentationの問題が起こりやすい。また、逆に、ノイズの影響が大きい計測データの場合に閾値を下げると、実際には一つの平面が複数の平面に分離されてしまうover-segmentationが起こる。
上述のHooverらの文献や、Iocchiらの文献などのように、ハフ変換によって平面を推定する手法の場合、under-segmentationの問題が非常に生じやすい。図３Ａ〜図３Ｄは、ハフ変換により平面を抽出する方法を説明する図であり、図３Ａは階段を示す図、図３Ｂは図３Ａに示す階段から得られた３次元距離データ、図３Ｃは図３Ｂの距離データをハフ変換してピークを求めた図、図３Ｄは図３Ｃに示すピークが示す平面と、実際の平面との比較を示す図である。図３Ａ〜Ｄに示すように、視野内に階段４１０を含む場合、その３次元データは、図３Ｂに示すものとなる。このデータの３点をランダムに選択して平面を求め、これを平面パラメータ空間に投票することでヒストグラムを生成し、図３Ｃに示すように支配的な平面がピークＰとして検出でき、このピークが示すパラメータによって決定される平面が視野内に存在する平面として推定される。
しかしながら、ハフ変換した後のデータから統計的に平面を推定すると、under-segmentationした結果が統計的に最も支配的な値となる。すなわち、図３Ｄに示すように、検出された平面４１１は、実際には全ての平面４１２、４１３、４１４を均した平面として求まってしまう。このように、ハフ変換は、視野内に含まれる支配的な平面を推定、検出することはできても、複数平面が存在する場合にそれらを精度よく検出することができない。
また、Jiangらの文献には、線分拡張法（scan line grouping）による平面抽出手法が開示されている。線分拡張法による平面検出では、まず、撮影された画像から３次元距離データを取得し、この３次元距離データにおいて、行方向または列方向のデータ列（image row：イメージロウ）毎に以下の処理を行う。例えば、画像内の行方向のデータ列において、３次元空間において同一の平面に属するならば同一の直線となるとことを利用し、同一平面に属するデータ点群から線分を生成する。そして、生成された線分群において、同一平面を構成する隣接する３本の線分を抽出して基準となる平面を求め、この基準となる平面に隣接する線分が同一平面に属する場合に当該隣接する線分によって基準となる平面の領域を拡大するとともに基準となる平面を更新することにより、平面を検出する方法である。
図４は、線分拡張法による平面検出処理を示すフローチャートである。図４に示すように、先ず、距離画像を入力し（ステップＳ４１）、距離画像を構成する行方向又は列方向の各データ列において同一の平面上にあると推定されるデータ点群から線分を生成する（ステップＳ４２）。そして、生成された線分群の中から平面の種となる領域（以下、領域種（seed region）という。）を検索し、該当する領域種を選択する（ステップＳ４３，Ｓ４４）。この選択においては、上下隣接する１ラインが同一平面にあることを条件とする。そして、選択した３本の線分からなる領域種が属する平面を３本の線分から平均して求めておく。
そして、ステップＳ３にて選択した領域種に隣接するデータ列において、その領域種と同一平面にある線分があるか否かを検索する。同一平面にあるか否かは、空間的な距離を比較することで判断する。同一平面にあると判断された線分が存在する場合には、当該線分をこの領域種の領域に追加し（領域拡張処理）、追加した線分を含めたものとして、元の平面を更新し（平面更新処理）、これらの処理を繰り返し行ことで、領域を拡張するとともに平面を更新する（ステップＳ４５）。更に、ステップＳ４３〜Ｓ４５の処理を、種となる領域が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結し（ステップＳ４６）、処理を終了する。
この図４に示す各処理について更に詳細に説明する。先ず、ステップＳ２に示す線分抽出方法について説明する。図５は、線分を抽出する処理を説明する図であって、図５Ａ〜図５Ｃは、その処理工程順に示す図である。図５Ａに示すように、先ず、与えられた複数のデータ点群４３０の両端４３０ａ，４３０ｂを結ぶ線分（弦）を生成する。そして、得られた線分４３１との距離が最も大きいデータ点を検索する。検索されたデータ点４３０ｃと線分４３１との距離ｄがある閾値を超える場合には、線分４３１を分割する処理を行う。具体的には、図５Ｂに示すように、線分４３１を、左端のデータ点４３０ａと分割点となるデータ点４３０ｃとを結ぶ線分４３１ａと、分割点４３０ｃと右端のデータ点４３０ｂとを結ぶ線分４３１ｂとに分割する。これを全ての点と線分との距離が閾値以下になるまで繰り返すことによって、与えられたデータにフィットした複数の線分を検出することができる。ここでは、図５Ｃに示すように、最終的に２箇所の分割点となるデータ点４３０ｃ，４３０ｄが選択され、線分４３１は、線分４３１ａ、４３１ｃ、４３１ｄの３本の線分に分割される。
図２５は、ステップＳ４５に示す領域拡張処理を説明するための図である。上述の線分抽出処理によって得られた線分を、種となる領域から順次統合していくことによって領域を分割することができる。例えば、図２５に示すように、画像３０内に複数の平面からなる階段３１が存在する場合、例えば太線で示す３２ａ〜３２ｃの３本の線分が領域種として選択されたとする。これら３本の線分３２ａ〜３２ｃからなる領域が領域種となる。先ず、この３つの線分３２ａ〜３２ｃにより１つの平面（基準平面）Ｐを求める。次に、領域種の最も外側の線分３２ａ又は３２ｃに領域種外にて隣接するそれぞれデータ列３３又は３４において、平面Ｐと同一の平面である線分を選択する。ここでは、線分３３ａが選択されるとする。次に、これら４本の線分群からなる平面Ｐ’を求め、基準平面Ｐを更新する。次に、線分３４ａが選択されれば、５本の線分群からなる平面Ｐ''を求め、平面Ｐ’を更新する。これを繰り返すことにより、階段３１の２段目の踏面が、破線で囲まれる平面４５として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像３０内から領域種となる３つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる３つの線分がなくなるまで図４のステップＳ４３〜Ｓ４５の処理を繰り返す。
しかしながら、この線分拡張法による平面検出アルゴリズムにおいても、線分抽出の際の閾値の決定が非常に困難であるため、計測データのノイズの影響を排除することが極めて難しい。図６は、２つの閾値設定をした場合における線分抽出処理の結果の相違を示す図である。図６Ａは、ノイズが少ない計測データ点群４５０の場合、図６Ｂは、ノイズが多い計測データ点群４６０の場合であり、それぞれの場合において、上述の線分分割の際の閾値として大きい値（Large threshold）を適用した場合と、小さい値（Small threshold）を適用した場合の結果を示す。
図６Ａに示すように、ノイズが少ない計測データ点群４５０から線分を抽出する場合、大きな閾値を設定するとunder-segmentationが生じてしまうため、小さい閾値のときの方がよい抽出結果を示す。一方、図６Ｂに示すように、ノイズが多いデータ点群４６０から線分を抽出する場合は、小さな閾値を設定すると、over-segmentationの問題が発生してしまうため、大きな閾値を設定する必要がある。すなわち、小さい閾値ではノイズの影響を受けて線分が過剰に細かく分割されてしまい、本来抽出すべき線分を抽出することができない。
一般に、ステレオビジョンなどの距離計測装置の場合、近くの計測データに関しては計測精度が高くノイズが少ない計測データを取得することができ、遠くの計測データに関しては、計測精度が低いためノイズが多い計測データとなってしまう。このため、距離に応じて適応的に閾値を決めることが望まれるが、環境による計測精度の違いの影響もあり、これを一意に決めるのは極めて困難である。
以上、まとめると、ランダマイズドハフ変換などによる平面検出は、支配的な平面を検出するには適しているが、階段など複数平面を含むデータから複数の平面を検出するには、under-segmentationの問題が生じてしまうという問題点があり、また線分拡張法を使用して平面を抽出する場合は、線分抽出の際の閾値の設定が難しく、over-segmentation及びunder-segmentationの問題が生じてしまい、いずれの方法においても、距離計測データから正確に平面を検出することが困難であるという問題点がある。
一般に、距離情報から平面を検出する手法は、以下の手順により構成されている。
１．３次元距離情報を取得する
２．複数の平面に分割する
３．各平面に関して、重心、平面方程式、境界等を計算する
４．２、３の処理を繰り返し行い、最適な分割方法を獲得する
例えば、図１Ａに示す床面に載置された階段を有する画像は、図１Ｂに示すように、４つの平面領域Ａ、Ｂ、Ｃ、Ｄに分割される。ここで、領域Ａは床部分、領域Ｂ、Ｃ、Ｄは階段部分を示す。
次に、３次元の距離情報から平面を検出する従来の手法を下記文献の技術を例にとって説明する。例えば、Hooverら（A. Hoover， G. Jean-Baptiste， X. Jiang， P.J. Flynn， H. Bunke， D. Goldgof， K. Bowyer， D. Eggert， A. Fitzgibbon， and R. Fisher.），「距離画像セグメンテーションアルゴリズムの実験的比較（An experimental comparison of range image segmentation algorithms）」，Transaction on Pattern Analysis Machine Intelligence，18(7)，1996 では、距離情報から参照点をランダムにサンプルし、その点から空間的に近傍の点を連結することによって平面を検出する手法を提案している。また、Iocchiら（L. Iocchi， K. Konolige，and M. Bajracharya.），「ステレオ画像を使用した環境平面における現実の視覚地図（Visually realistic mapping of planar environment with stereo）」Int. Symposium on Experimental Robotics (ISER)，2000 では、距離情報からハフ変換によって統計的に平面を検出する手法を提案している。更に、Okadaら（K. Okada，S. Kagami，M. Inaba and H. Inoue.「平面セグメントの検索アルゴリズム，実装，アプリケーション（Plane Segment Finder: Algorithm，Implementation，and Applications）」Int. Conf. on Robotics and Automation (ICRA), 2001 では、距離情報からランダムに選んだ３点を平面方程式に当てはめ、統計的に平面を検出するランダマイズドハフ変換による手法を提案している。更にまた、Jiangら（X.-Y. Jiang and H. Bunke.）「線分拡張法によって距離画像を平面領域に高速に分割する手法（Fast segmentation of range images into planar regions by scan line grouping）」Machine Vision and Applications，7(2)，P115 122，1994 では、距離画像中の列に並んだ点群はそれらの点が３次元空間で同一平面上にあるならば、空間内で直線となることを利用し、線分拡張法(scan line grouping)による手法を提案している。
ところで、上述の各文献などのように、距離情報を使用した平面検出において重大な問題となるのが、ノイズに起因する、under-segmentation, over-segmentationの問題である。
under-segmentationとは、実際は複数の平面が存在するのにも拘わらず、ノイズの影響などによりそれらを例えば平均化したような１つの平面として認識してしまうことをいい、over-segmentationとは、実際は１つの同一平面であるのにも拘わらず、ノイズの影響などにより複数の異なる平面として認識してしまうものである。
図２に示すように、カメラ４０１Ｒ／Ｌにより取得された距離画像には、複数の踏面や、側面、床面などが含まれる。例えば図２の上図に示すように、例えばカメラ４０１Ｒ／Ｌの視野内に階段４００が存在する場合、距離画像内には複数の平面を含む。したがって、図２下図に示すように、ｘ−ｚ平面であれば、踏面４０２、側面４０３などの複数の平面を含むが、under-segmentationにより、これら複数の平面を区別できず、一つの平面４０３として検出してしまう。平面検出器は、一般に、平面の検出要求精度に比べてノイズの影響が大きい計測データから平面を検出するため、これを設計する際には、複数平面に分離する為の閾値を甘くしなければならず、従ってこのようなunder-segmentationの問題が起こりやすい。また、逆に、ノイズの影響が大きい計測データの場合に閾値を下げると、実際には一つの平面が複数の平面に分離されてしまうover-segmentationが起こる。
上述のHooverらの文献や、Iocchiらの文献などのように、ハフ変換によって平面を推定する手法の場合、under-segmentationの問題が非常に生じやすい。図３Ａ〜図３Ｄは、ハフ変換により平面を抽出する方法を説明する図であり、図３Ａは階段を示す図、図３Ｂは図３Ａに示す階段から得られた３次元距離データ、図３Ｃは図３Ｂの距離データをハフ変換してピークを求めた図、図３Ｄは図３Ｃに示すピークが示す平面と、実際の平面との比較を示す図である。図３Ａ〜Ｄに示すように、視野内に階段４１０を含む場合、その３次元データは、図３Ｂに示すものとなる。このデータの３点をランダムに選択して平面を求め、これを平面パラメータ空間に投票することでヒストグラムを生成し、図３Ｃに示すように支配的な平面がピークＰとして検出でき、このピークが示すパラメータによって決定される平面が視野内に存在する平面として推定される。
しかしながら、ハフ変換した後のデータから統計的に平面を推定すると、under-segmentationした結果が統計的に最も支配的な値となる。すなわち、図３Ｄに示すように、検出された平面４１１は、実際には全ての平面４１２、４１３、４１４を均した平面として求まってしまう。このように、ハフ変換は、視野内に含まれる支配的な平面を推定、検出することはできても、複数平面が存在する場合にそれらを精度よく検出することができない。
また、Jiangらの文献には、線分拡張法（scan line grouping）による平面抽出手法が開示されている。線分拡張法による平面検出では、まず、撮影された画像から３次元距離データを取得し、この３次元距離データにおいて、行方向または列方向のデータ列（image row：イメージロウ）毎に以下の処理を行う。例えば、画像内の行方向のデータ列において、３次元空間において同一の平面に属するならば同一の直線となるとことを利用し、同一平面に属するデータ点群から線分を生成する。そして、生成された線分群において、同一平面を構成する隣接する３本の線分を抽出して基準となる平面を求め、この基準となる平面に隣接する線分が同一平面に属する場合に当該隣接する線分によって基準となる平面の領域を拡大するとともに基準となる平面を更新することにより、平面を検出する方法である。
図４は、線分拡張法による平面検出処理を示すフローチャートである。図４に示すように、先ず、距離画像を入力し（ステップＳ４１）、距離画像を構成する行方向又は列方向の各データ列において同一の平面上にあると推定されるデータ点群から線分を生成する（ステップＳ４２）。そして、生成された線分群の中から平面の種となる領域（以下、領域種（seed region）という。）を検索し、該当する領域種を選択する（ステップＳ４３，Ｓ４４）。この選択においては、上下隣接する１ラインが同一平面にあることを条件とする。そして、選択した３本の線分からなる領域種が属する平面を３本の線分から平均して求めておく。
そして、ステップＳ３にて選択した領域種に隣接するデータ列において、その領域種と同一平面にある線分があるか否かを検索する。同一平面にあるか否かは、空間的な距離を比較することで判断する。同一平面にあると判断された線分が存在する場合には、当該線分をこの領域種の領域に追加し（領域拡張処理）、追加した線分を含めたものとして、元の平面を更新し（平面更新処理）、これらの処理を繰り返し行ことで、領域を拡張するとともに平面を更新する（ステップＳ４５）。更に、ステップＳ４３〜Ｓ４５の処理を、種となる領域が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結し（ステップＳ４６）、処理を終了する。
この図４に示す各処理について更に詳細に説明する。先ず、ステップＳ２に示す線分抽出方法について説明する。図５は、線分を抽出する処理を説明する図であって、図５Ａ〜図５Ｃは、その処理工程順に示す図である。図５Ａに示すように、先ず、与えられた複数のデータ点群４３０の両端４３０ａ，４３０ｂを結ぶ線分（弦）を生成する。そして、得られた線分４３１との距離が最も大きいデータ点を検索する。検索されたデータ点４３０ｃと線分４３１との距離ｄがある閾値を超える場合には、線分４３１を分割する処理を行う。具体的には、図５Ｂに示すように、線分４３１を、左端のデータ点４３０ａと分割点となるデータ点４３０ｃとを結ぶ線分４３１ａと、分割点４３０ｃと右端のデータ点４３０ｂとを結ぶ線分４３１ｂとに分割する。これを全ての点と線分との距離が閾値以下になるまで繰り返すことによって、与えられたデータにフィットした複数の線分を検出することができる。ここでは、図５Ｃに示すように、最終的に２箇所の分割点となるデータ点４３０ｃ，４３０ｄが選択され、線分４３１は、線分４３１ａ、４３１ｃ、４３１ｄの３本の線分に分割される。
図２５は、ステップＳ４５に示す領域拡張処理を説明するための図である。上述の線分抽出処理によって得られた線分を、種となる領域から順次統合していくことによって領域を分割することができる。例えば、図２５に示すように、画像３０内に複数の平面からなる階段３１が存在する場合、例えば太線で示す３２ａ〜３２ｃの３本の線分が領域種として選択されたとする。これら３本の線分３２ａ〜３２ｃからなる領域が領域種となる。先ず、この３つの線分３２ａ〜３２ｃにより１つの平面（基準平面）Ｐを求める。次に、領域種の最も外側の線分３２ａ又は３２ｃに領域種外にて隣接するそれぞれデータ列３３又は３４において、平面Ｐと同一の平面である線分を選択する。ここでは、線分３３ａが選択されるとする。次に、これら４本の線分群からなる平面Ｐ’を求め、基準平面Ｐを更新する。次に、線分３４ａが選択されれば、５本の線分群からなる平面Ｐ''を求め、平面Ｐ’を更新する。これを繰り返すことにより、階段３１の２段目の踏面が、破線で囲まれる平面４５として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像３０内から領域種となる３つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる３つの線分がなくなるまで図４のステップＳ４３〜Ｓ４５の処理を繰り返す。
しかしながら、この線分拡張法による平面検出アルゴリズムにおいても、線分抽出の際の閾値の決定が非常に困難であるため、計測データのノイズの影響を排除することが極めて難しい。図６は、２つの閾値設定をした場合における線分抽出処理の結果の相違を示す図である。図６Ａは、ノイズが少ない計測データ点群４５０の場合、図６Ｂは、ノイズが多い計測データ点群４６０の場合であり、それぞれの場合において、上述の線分分割の際の閾値として大きい値（Large threshold）を適用した場合と、小さい値（Small threshold）を適用した場合の結果を示す。
図６Ａに示すように、ノイズが少ない計測データ点群４５０から線分を抽出する場合、大きな閾値を設定するとunder-segmentationが生じてしまうため、小さい閾値のときの方がよい抽出結果を示す。一方、図６Ｂに示すように、ノイズが多いデータ点群４６０から線分を抽出する場合は、小さな閾値を設定すると、over-segmentationの問題が発生してしまうため、大きな閾値を設定する必要がある。すなわち、小さい閾値ではノイズの影響を受けて線分が過剰に細かく分割されてしまい、本来抽出すべき線分を抽出することができない。
一般に、ステレオビジョンなどの距離計測装置の場合、近くの計測データに関しては計測精度が高くノイズが少ない計測データを取得することができ、遠くの計測データに関しては、計測精度が低いためノイズが多い計測データとなってしまう。このため、距離に応じて適応的に閾値を決めることが望まれるが、環境による計測精度の違いの影響もあり、これを一意に決めるのは極めて困難である。
以上、まとめると、ランダマイズドハフ変換などによる平面検出は、支配的な平面を検出するには適しているが、階段など複数平面を含むデータから複数の平面を検出するには、under-segmentationの問題が生じてしまうという問題点があり、また線分拡張法を使用して平面を抽出する場合は、線分抽出の際の閾値の設定が難しく、over-segmentation及びunder-segmentationの問題が生じてしまい、いずれの方法においても、距離計測データから正確に平面を検出することが困難であるという問題点がある。
本発明は、このような従来の実情に鑑みて提案されたものであり、計測ノイズを含む距離データからノイズに対してロバストにかつ複数平面を同時に正確に検出することができる平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置を提供することを目的とする。
上述した目的を達成するために、本発明に係る平面検出装置は、３次元の距離データから平面を検出する平面検出装置において、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
本発明においては、線分抽出手段は、３次元の距離データが同一平面上にある場合同一直線上に並ぶことを利用して線分を抽出するが、この際、ノイズなどの影響により距離データ点の分布に違いが生じるため、この距離データの分布に応じて適応的に線分を抽出する（Adaptive Line Fitting）ことにより、ノイズに対してロバストに、精確な線分抽出を可能とし、抽出された多数の線分から線分拡張法により平面を求めるため、ノイズの影響などにより、本来複数平面が存在するのに１つの平面としたり、１つの平面しか存在しないのに複数平面としたりすることなく精確に平面抽出することができる。
また、上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定することができ、距離データ点の３次元空間における距離に基づき一旦距離データ点群を抽出しておき、データ点の分布に基づき再度同一平面上にあるか否かを推定することにより精確に線分抽出することができる。
更に、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割することができ、距離データ点の分布に偏りがある場合には、抽出した距離データ点群は同一の平面上にないと判断して着目点にて分割することができる。
更にまた、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することができ、例えば抽出したデータ点群の端点を結ぶ線分を第１の線分とし、上記距離が大きい点が存在する場合には、例えば最小二乗法により第２の線分を生成し、この第２の線分において一方側に連続して複数のデータ点が存在する場合には、データ点群は例えば線分に対してジグザグな形などをとっていることが想定でき、従って抽出したデータ点群には偏りがあると判断して、上記着目点などにてデータ点群を分割することができる。
また、上記平面領域拡張手段は、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力することができ、同一平面に属するとされる線分により平面領域拡張処理及び平面更新処理を行うことができる。
更に、上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有することができ、更新済平面はそれに属する全線分の平均した平面として得られているため、これから大きく外れた距離データ点を除いたデータ点群から再度平面を求めることで、よりノイズなどの影響を低減した検出結果を得ることができる。
更に、上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定することができ、例えば平面方程式の２乗平均誤差などに基づきノイズの影響であるのか、異なる平面なのかを判別して更に正確に平面検出することができる。
本発明に係る平面検出方法は、３次元の距離データから平面を検出する平面検出方法において、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、上記線分抽出工程にて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、上記線分抽出工程では、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
本発明に係るロボット装置は、自律的に行動するロボット装置において、３次元の距離データを取得する距離計測手段と、３次元の距離データから平面を検出する平面検出装置と、上記平面検出装置による平面検出結果に基づき行動を制御する行動制御手段とを有し、上記平面検出装置は、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
また、対象物に対して模様を照射する照射手段などの模様付与手段を有することができ、距離計測手段がステレオカメラなどにより視差を利用して距離画像を取得するものである場合、観察対象となる階段、床面などの対象物が模様（テクスチャ）がない又は不十分であると、うまく距離画像を得ることができないため、この模様付与手段により模様を付与して精確な距離画像を取得することができる。
本発明に係る平面検出装置及び方法によれば、線分拡張法により平面検出する際、距離データ点の分布に応じて適応的に線分を抽出することで、ノイズを含む距離データであっても精確に線分抽出することができ、この線分を使用して線分拡張法により平面検出することで、極めて精確に平面を検出することができる。
また、本発明に係るロボット装置は、上述の平面検出装置を搭載することにより、ロボット装置が具備する距離計測手段にてノイズを含む距離データが取得されても精確に平面検出することができ、ロボット装置の周囲環境内に存在する階段を検出して昇降動作をさせたり、床面の段差などを認識して段差がある床面上を移動することなどが可能となり、更にエンターテイメント性が高まる。
本発明の更に他の目的、本発明によって得られる利点は、以下において図面を参照して説明される実施に形態から一層明らかにされるであろう。
上述した目的を達成するために、本発明に係る平面検出装置は、３次元の距離データから平面を検出する平面検出装置において、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
本発明においては、線分抽出手段は、３次元の距離データが同一平面上にある場合同一直線上に並ぶことを利用して線分を抽出するが、この際、ノイズなどの影響により距離データ点の分布に違いが生じるため、この距離データの分布に応じて適応的に線分を抽出する（Adaptive Line Fitting）ことにより、ノイズに対してロバストに、精確な線分抽出を可能とし、抽出された多数の線分から線分拡張法により平面を求めるため、ノイズの影響などにより、本来複数平面が存在するのに１つの平面としたり、１つの平面しか存在しないのに複数平面としたりすることなく精確に平面抽出することができる。
また、上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定することができ、距離データ点の３次元空間における距離に基づき一旦距離データ点群を抽出しておき、データ点の分布に基づき再度同一平面上にあるか否かを推定することにより精確に線分抽出することができる。
更に、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割することができ、距離データ点の分布に偏りがある場合には、抽出した距離データ点群は同一の平面上にないと判断して着目点にて分割することができる。
更にまた、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することができ、例えば抽出したデータ点群の端点を結ぶ線分を第１の線分とし、上記距離が大きい点が存在する場合には、例えば最小二乗法により第２の線分を生成し、この第２の線分において一方側に連続して複数のデータ点が存在する場合には、データ点群は例えば線分に対してジグザグな形などをとっていることが想定でき、従って抽出したデータ点群には偏りがあると判断して、上記着目点などにてデータ点群を分割することができる。
また、上記平面領域拡張手段は、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力することができ、同一平面に属するとされる線分により平面領域拡張処理及び平面更新処理を行うことができる。
更に、上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有することができ、更新済平面はそれに属する全線分の平均した平面として得られているため、これから大きく外れた距離データ点を除いたデータ点群から再度平面を求めることで、よりノイズなどの影響を低減した検出結果を得ることができる。
更に、上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定することができ、例えば平面方程式の２乗平均誤差などに基づきノイズの影響であるのか、異なる平面なのかを判別して更に正確に平面検出することができる。
本発明に係る平面検出方法は、３次元の距離データから平面を検出する平面検出方法において、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、上記線分抽出工程にて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、上記線分抽出工程では、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
本発明に係るロボット装置は、自律的に行動するロボット装置において、３次元の距離データを取得する距離計測手段と、３次元の距離データから平面を検出する平面検出装置と、上記平面検出装置による平面検出結果に基づき行動を制御する行動制御手段とを有し、上記平面検出装置は、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。
また、対象物に対して模様を照射する照射手段などの模様付与手段を有することができ、距離計測手段がステレオカメラなどにより視差を利用して距離画像を取得するものである場合、観察対象となる階段、床面などの対象物が模様（テクスチャ）がない又は不十分であると、うまく距離画像を得ることができないため、この模様付与手段により模様を付与して精確な距離画像を取得することができる。
本発明に係る平面検出装置及び方法によれば、線分拡張法により平面検出する際、距離データ点の分布に応じて適応的に線分を抽出することで、ノイズを含む距離データであっても精確に線分抽出することができ、この線分を使用して線分拡張法により平面検出することで、極めて精確に平面を検出することができる。
また、本発明に係るロボット装置は、上述の平面検出装置を搭載することにより、ロボット装置が具備する距離計測手段にてノイズを含む距離データが取得されても精確に平面検出することができ、ロボット装置の周囲環境内に存在する階段を検出して昇降動作をさせたり、床面の段差などを認識して段差がある床面上を移動することなどが可能となり、更にエンターテイメント性が高まる。
本発明の更に他の目的、本発明によって得られる利点は、以下において図面を参照して説明される実施に形態から一層明らかにされるであろう。
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、複数平面を同時に精度よく検出可能な平面検出装置を搭載したロボット装置に適用したものである。
本実施の形態における平面検出装置は、ステレオビジョンなどにより得られた距離情報（距離データ）から複数の平面を抽出することができ、計測ノイズに対してロバストな検出を行うことが可能であり、例えば、ロボット装置に搭載すれば、ロボット装置が自身の周囲の環境を正確に認識することができ、例えば階段などの複数平面を含む物体、周囲の状況を認識し、この認識結果に応じて自律的に移動したり、行動したりすることができる。
本実施の形態においては、まず、このようなロボット装置の一例として２足歩行タイプのロボット装置を例にとって説明する。このロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボット装置である。なお、ここでは、２足歩行型のロボット装置を例にとって説明するが、２足歩行のロボット装置に限らず、４足又は車輪等により移動可能なロボット装置に適用できることはいうまでもない。
図７は、本実施の形態におけるロボット装置の概観を示す斜視図である。図７に示すように、ロボット装置２０１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。
このロボット装置２０１が具備する関節自由度構成を図８に模式的に示す。頭部ユニット２０３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
また、上肢を構成する各々の腕部ユニット２０４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール輪１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置２０１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
また、体幹部ユニット２０２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
また、下肢を構成する各々の脚部ユニット２０５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足底１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置２０１の股関節位置を定義する。人体の足底１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置２０１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
以上を総括すれば、ロボット装置２０１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置２０１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
上述したようなロボット装置２０１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うこと等の要請から、アクチュエータは小型且つ軽量であることが好ましい。
このようなロボット装置は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット２０２等に備える。図９は、ロボット装置２０１の制御システム構成を示す模式図である。図９に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール２００と、アクチュエータ３５０の駆動等、ロボット装置２０１の全身協調運動を制御する運動制御モジュール３００とで構成される。
思考制御モジュール２００は、情緒判断や感情表現に関する演算処理を実行するＣＰＵ（Central Processing Unit）２１１や、ＲＡＭ（Random Access Memory）２１２、ＲＯＭ（Read Only Memory）２１３及び外部記憶装置（ハード・ディスク・ドライブ等）２１４等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。
この思考制御モジュール２００は、画像入力装置２５１から入力される画像データや音声入力装置２５２から入力される音声データ等、外界からの刺激等に従って、ロボット装置２０１の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット装置２０１の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置２５１は、例えばＣＣＤ（Charge Coupled Device）カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。また、音声入力装置２５２は、例えばマイクロホンを複数備えている。
思考制御モジュール２００は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール３００に対して指令を発行する。
一方の運動制御モジュール３００は、ロボット装置２０１の全身協調運動を制御するＣＰＵ３１１や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。
この運動制御モジュール３００には、図８に示したロボット装置２０１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インタフェース（Ｉ／Ｆ）３１０経由で接続されている。ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組み合わせによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。
思考制御モジュール２００と運動制御モジュール３００は、共通のプラットフォーム上で構築され、両者間はバス・インタフェース２１０，３１０を介して相互接続されている。
運動制御モジュール３００では、思考制御モジュール２００から指示された行動を体現すべく、各アクチュエータ３５０による全身協調運動を制御する。すなわち、ＣＰＵ３１１は、思考制御モジュール２００から指示された行動に応じた動作パターンを外部記憶装置３１４から取り出し、又は、内部的に動作パターンを生成する。そして、ＣＰＵ３１１は、指定された動作パターンに従って、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各アクチュエータ３５０に転送する。
また、ＣＰＵ３１１は、姿勢センサ３５１の出力信号によりロボット装置２０１の体幹部ユニット２０２の姿勢や傾きを検出するとともに、各接地確認センサ３５２，３５３の出力信号により各脚部ユニット２０５Ｒ／Ｌが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット装置２０１の全身協調運動を適応的に制御することができる。更に、ＣＰＵ３１１は、ＺＭＰ位置が常にＺＭＰ安定領域の中心に向かうように、ロボット装置２０１の姿勢や動作を制御する。
また、運動制御モジュール３００は、思考制御モジュール２００において決定された意思通りの行動がどの程度発現されたか、すなわち処理の状況を、思考制御モジュール２００に返すようになっている。このようにしてロボット装置２０１は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
このようなロボット装置においては、頭部ユニット２０３にステレオビジョンシステムを搭載し、外界の３次元距離情報を取得することができる。次に、このようなロボット装置などに好適に搭載されるものであって、ステレオビジョンによる３次元距離情報を利用した本実施の形態における平面検出装置について説明する。なお、距離情報としては、レーザレンジファインダ（レーザ距離計測計）などによる距離情報を使用してもよいことは勿論である。
本実施の形態における平面検出装置は、線分拡張法により、視野内において支配的な平面だけでなく、例えば階段など複数の平面が存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさせることにより計測ノイズに対してロバストな平面検出結果を得ることができるものである。
図１０は、本実施の形態における平面検出装置を示す機能ブロック図である。図１０に示すように、平面検出装置１は、３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）２と、３次元の距離データからなる距離画像に存在する平面を線分拡張法により検出する平面検出部３とを有する。平面検出部３は、画像を構成する距離データ点から同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部４と、画像内に含まれる、線分抽出部４よって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部５とを有する。領域拡張部５は、線分群から同一平面上に存在すると推定される任意の３本の線分を選択し、これらから基準平面を求める。そして、選択した３本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新するとともに基準平面の領域を拡張する。
ステレオビジョンシステム２は、例えばロボット装置２０１の画像入力装置２５１により取得された画像から距離画像を生成するものであり、外界を観測した結果、両眼の視差によって推定される３次元距離データＤ１を線分抽出部４に出力する。
線分抽出部４は、その距離画像における列または行毎の各データ列において、３次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて１以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群Ｄ２を領域拡張部５に出力する。
領域拡張部５は、この線分群Ｄ２において、同一の平面に属すると推定される線分を３本選択し、これらから基準平面としての種となる平面を求める。この種となる平面の領域（領域種：seed region）に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群Ｄ３を出力する。
ロボット装置２０１は、障害物回避や階段昇降など平面の情報が必要なとき、または定期的にこれらの処理を行うことによって、階段や床面、壁といった歩行に重要な平面の情報を取得する。
次に、平面検出装置１を構成するステレオビジョンシステム２について更に詳細に説明する。ステレオビジョンシステム２は、人間の両眼に相当する左右２つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。
図１１は、ロボット装置２０１が外界を撮影している様子を示す模式図である。床面をｘ−ｙ平面とし、高さ方向をｚ方向としたとき、図１１に示すように、画像入力部（ステレオカメラ）を頭部ユニット２０３に有するロボット装置２０１の視野範囲は、ロボット装置２０１の前方の所定範囲となる。
ロボット装置２０１は、上述したＣＰＵ２１１において、画像入力装置２５１からのカラー画像及び視差画像と、各アクチュエータ３５０の全ての関節角度等のセンサデータとなどが入力されてソフトウェア構成を実現する。
本実施の形態のロボット装置２０１におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。なお、ロボット装置の位置を示す座標として、例えば、ランドマーク等の特定の物体等に基づく所定位置を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心（座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との２つの座標を使用する。
ステレオビジョンシステム２では、カラー画像及びステレオカメラによる視差画像などの画像データが撮像された時間において、センサデータから割り出した関節角を使用してロボット装置２０１が中心に固定されたロボット中心座標系を頭部ユニット２０３に設けられた画像入力装置２５１の座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列とこれに対応する３次元距離データからなる距離画像を平面検出部３へ出力する。
ここで、本実施の形態における平面検出装置は、上述したハフ変換のように、取得した画像に含まれる支配的な平面を検出するのみならず、階段など複数平面が含まれている場合であっても平面を検出可能とするため、線分拡張法により平面を検出する。そして、この際、距離データ点の分布に応じて線分を生成することにより、計測ノイズに対してロバストな検出結果を得ることができるものである。以下では、本実施の形態における平面検出装置を搭載したロボット装置が視野内に含まれる階段ＳＴを検出する場合を例にとって説明する。
階段としては、例えば図１２、図１３に示すようなものがある。図１２Ａ、図１３Ａは、階段を正面から見た図、図１２Ｂ）、図１３Ｂは、階段を側面から見た図、図１２Ｃ、図１３Ｃは、階段を斜めから見た図である。
ここで、本明細書においては、人間、ロボット装置などが階段を昇降するために使用する面（足又は可動脚部を載せる面）を踏面といい、一の踏面からその次の踏面までの高さ（１段の階段の高さ）を蹴り上げということとする。また、階段は、地面に近い方から１段目、２段目とカウントすることとする。
図１２に示す階段ＳＴ１は、段数が３段の階段であり、蹴り上げ４ｃｍ、１，２段面の踏面の大きさは幅３０ｃｍ、奥行き１０ｃｍ、最上段である３段目の踏面のみ、幅３０ｃｍ、奥行き２１ｃｍとなっている。また、図１３に示す階段ＳＴ２も段数が３段の階段であり、蹴り上げ３ｃｍ、１，２段面の踏面の大きさは幅３３ｃｍ、奥行き１２ｃｍ、最上段である３段目の踏面のみ、幅３３ｃｍ、奥行き３２ｃｍとなっている。
図１４乃至図１６は、図１３に示す階段ＳＴ２を示し、図１４Ａ、図１５Ａ、図１６Ａは、図１３に示す階段をステレオビジョンシステムによってそれぞれ前方、側方、斜め前方から撮影した場合の画像を示す模式図、図１４Ｂ乃至図１６Ｄは、図１４Ａ、図１５Ａ、図１６Ａに示す画像から取得した３次元の距離データを示す図である。
図１４Ａに示すように、正面から階段ＳＴ２を撮影した場合、３次元の距離データは、図１４Ｂ乃至図１４Ｄのようになる。図１４Ｂにおいて、横軸をｙ方向、縦軸をｘ方向とし、ｚ軸方向（高さ方向）の大きさは、ロボット装置２０１の接地面を０とし、高さが高くなるほど白に近づくような濃淡値で示している。すなわち、濃淡（濃淡値）が同様のデータ点は同一高さにあることを示し、図１４Ｂに示すように、階段ＳＴ２においても１段目より２段目、２段目より３段目の踏面にあたる領域のデータ点の濃淡が薄くなっている。また、距離データが図示されている略台形領域がロボット装置が撮影可能な範囲（視野範囲）を示す。また、同図において、距離データ点は略４段階の濃淡に分かれているが、ｚ方向が最も小さい領域に対応する最も濃淡が濃い箇所は、床面を示している。また、図１４Ｃは、横軸をｙ方向、縦軸をｚ方向、ｘ方向を色の濃淡で示すものである。この図では、ｘ方向の距離が大きくなるほど濃淡が薄くなるように表現されている。図１４Ｄは、横軸をｘ方向、縦軸をｚ方向とし、ｙ方向をその距離に応じて濃淡で表現している。
また、ロボット装置２０１が階段ＳＴ２の側面を撮影した場合は、図１５Ａ乃至図１５Ｄに示すように、ｘ軸が大きい上方側の領域に存在するデータ点が、高さが０と同様の濃淡を示しており、階段ＳＴ２の奥の床面を計測した結果であることを示している。また、図１６Ａ乃至図１６Ｄに示す斜め方向からの撮像においても、床面及び１〜３段目の踏面を示す４つの領域が、高さの違いに応じて異なる濃淡で示され、はっきり区別できていることを示している。
ここで、このような３次元距離データをステレオカメラによって取得するためには、階段ＳＴ２の表面に模様（テクスチャ）が必要となる。すなわち、２台のカメラによる視差により得ることができるため、模様がないものは視差が算出できず、正確に距離を計測することができない。すなわち、ステレオビジョンシステムにおける距離データの計測精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
そこで、図１７に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステムを構成するステレオカメラ１１Ｒ／Ｌを備えるとともに、例えば同じく頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源１２を設ける。この光源１２は、模様がない階段ＳＴ３、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射（照射）し、ランダムなパターンＰＴを付与する模様付与手段として作用する。なお、ランダムパターンＰＴを形成して距離画像を取得できるものであれば、ランダムパターンＰＴを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載されるＣＣＤカメラなどにおいては観測可能なパターンを付与することができる。
次に、平面検出装置１の平面検出部３について説明する。この平面検出部３は、線分拡張法を使用して平面を検出するものであり、図１８は、線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出では、図１８に示すように、まず、焦点Ｆから撮影された画像１１において、行方向または列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row：イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分からなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。
図１９は、線分拡張法による平面検出処理を示すフローチャートである。図１９に示すように、先ず、距離画像を入力し（ステップＳ１）、距離画像の行方向（又は列方向）の各画素列において同一平面に属すると推定されるデータ点から線分を求める（ステップＳ２）。そして、これらの線分群の中から同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める（ステップＳ３）。このステップＳ３では、まず、平面の種となる領域（以下、領域種（seed region）という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の１ラインを含む３本の線分が同一平面にあることを条件とする。ここで、選択した３本の線分からなる領域種が属する平面を基準平面とし、３本の線分から平均して求まる平面を求めておく。また、３本の線分からなる領域を基準平面領域とする。
そして、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し（領域拡張処理）、追加した線分を含めたものとして上記基準平面を更新し（平面更新処理）、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域（３本の線分）が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結する。そして、本実施の形態においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップＳ４として更に設け、最終的な平面とするが、詳細は後述する。
ここで、３次元距離データから線分を検出し、これを同一平面毎にまとめた領域を１つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本実施の形態においては、ステップＳ２における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しようとしても、距離データの精度に応じて閾値を変更しなければ、over-segmentation又はunder-segmentationなどもの問題が生じてしまう。そこで、本実施の形態においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。
以下、図１９に示す線分拡張法による平面検出方法について更に詳細に説明する。線分抽出器(Line Extraction)４は、上述したように、ステレオビジョンシステム２からの３次元距離画像を入力とし、距離画像の各列または各行毎に３次元空間内で同一平面上にあると推定される線分を検出する。この線分抽出において、計測ノイズなどによる、上述したover-segmentationやunder-segmentationの問題、すなわち、本来は複数の平面であるのに１つの平面として認識してしまったり、本来は１つの平面であるのに、複数の平面として認識してしまったりする問題を回避するため、データ点の分布に応じて適応的に線分フィッティングさせるアルゴリズム（Adaptive Line Fitting）を導入する。Adaptive Line Fittingは、線分抽出部４において、先ず比較的大きい閾値を使用して大まかに第１の線分としての線分を抽出し、次に抽出された第１の線分に属するデータ点群から後述する最小二乗法によって得られる第２の線分としての線分に対する該データ点群の分布を解析する。すなわち、同一平面上に存在するか否かを大まかに推定してデータ点群を抽出し、抽出したデータ点群におけるデータ点の分布の偏りがあるか否かを解析して同一平面上に存在しているか否かを再度推定する。
本実施の形態においては、このデータ点の分布を解析し、データ点群が後述するジグザグ形（zig-zag-shape）に当てはまる場合には、分布に偏りがあるとしてデータ点群を分割する処理を行い、これを繰り返すことによって、データ点群に含まれるノイズに対して適応的に線分の抽出を行うアルゴリズムを使用するものとする。
図２０は、線分抽出部４における処理、すなわち、図１９におけるステップＳ２の処理の詳細を示すフローチャートである。まず、線分抽出部４には、距離データが入力される。入力された距離データのうち、例えば行方向の画素列（データ点列）において、３次元空間上で同一平面上に存在すると推定されるデータ点群を抽出する。３次元空間上で同一平面上に存在すると推定されるデータ点群は、例えば隣接するデータ点の距離が、例えば６ｃｍ以下など、データ点間の３次元空間における距離が所定の閾値以下のものからなるデータ点の集合などとすることができ、これをデータ点群（Ｐ［0・・・n-1］）として抽出する（ステップＳ１１）。そして、このデータ点群Ｐ［0・・・n-1］に含まれるサンプル数ｎが処理に最低限必要なサンプル数（必要最小値）min_nより多いか否かをチェックし（ステップＳ１２）、データ数ｎが必要最小値min_nより少ない場合（Ｓ２：ＹＥＳ）には、検出結果として空集合を出力して処理を終了する。
一方、サンプル数ｎが必要最小値min_n以上である場合（Ｓ２：ＮＯ）、データ点群Ｐ［0・・・n-1］の一方の端点Ｐ［0］と他方の端点Ｐ［n-1］とを結ぶ線分（弦）Ｌ１を第１の線分として生成する。そして、データ点群Ｐ［0・・・n-1］から、この線分Ｌ１との距離が最も大きいデータ点を着目点brkとして検索し、その距離distを算出する（ステップＳ１３）。最大距離distがデータ点群分割の閾値max_dより大きい場合には（Ｓ１４：ＹＥＳ）、データ点群データ点群Ｐ［0・・・n-1］を着目点（分割点）brkにて２つのデータ点群ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する（ステップＳ１８）。
一方、最大距離distがデータ点群分割の閾値max_dより小さい場合には（Ｓ１４：ＮＯ）、データ点群Ｐ［0・・・n-1］から後述する最小二乗法によって最適な線分の方程式lineを求め（ステップＳ１５）、この方程式lineが示す線分Ｌ２を第２の線分として生成する。そして、データ点群Ｐ［0・・・n-1］がこの線分Ｌ２に対して後述するZig-Zag-Shapeであるかどうかを調べる（ステップＳ１６）、Zig-Zag-Shapeでない場合（S１６：ＮＯ）、得られた線分の方程式lineを線分抽出結果リストに追加し（ステップＳ１７）、処理を終了する。
また、ステップＳ１６においてステップＳ１５で求めた線分がZig-Zag-Shapeである判断された場合（Ｓ１６：ＹＥＳ）の場合、上述のステップＳ１４と同様、ステップＳ１８に進み、ステップＳ１３において距離distを求めた着目点brkにてデータ点群を２つのデータ点群Ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する。このステップＳ１８にて２つのデータ点群が得られた場合には、それぞれを再帰的に再度ステップＳ１１からの処理を行う。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すなわち全てのデータ点群がステップＳ１７を経るまで処理を繰り返し、これにより、全ての線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群Ｐ［0・・・n-1］からノイズの影響を排除し複数の線分からなる線分群を精度よく検出することができる。
なお、ステップＳ１３にてデータ点群Ｐ［0・・・n-1］の端点を結ぶ線分Ｌ１を生成するものとして説明したが、例えばデータ点群Ｐ［0・・・n-1］の分布、性質など必要に応じてデータ点群Ｐ［0・・・n-1］から最小二乗により線分Ｌ１を求めてもよい。また、本実施の形態においては、着目点brkは、端点を結んだ線分Ｌ１との距離が最大の点１つとしているが、例えば、上記のように最小二乗により求めた線分との距離が最大の点としたり、距離がデータ点群分割の閾値max_d以上のものが複数ある場合はそれら全ての点又は選択した１つ以上にてデータ点群Ｐ［0・・・n-1］を分割するようにしてもよい。
次に、ステップＳ１５における最小二乗による線分生成方法（Least-Squares Line Fitting）について説明する。あるｎ個のデータ点群Ｐ［0・・・n-1］が与えられたとき、データ点群に最もフィットした直線の方程式を求める方法を示す。直線の方程式のモデルを下記式（１）で表す。
この場合、ｎ個のデータ点群Ｐ［0・・・n-1］の１点（ｘｉ,ｙｉ）において、直線方程式のモデルとデータ点との誤差の総和は下記式（２）で表すことができる。
データ点群に最もフィットした直線は、上記式（２）の誤差の総和を最小化することによって求められる。上記式（２）を最小にするα及びｄは、データ点群Ｐの平均及び分散共分散行列を用いて下記（３）のように求めることができる。
次に、ステップＳ１６におけるジグザグ形（Zig-Zag-Shape）判別方法について説明する。このZig-Zag-Shape判別では、あるｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α，ｄ）、ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、そのデータ点群Ｐ［0・・・n-1］が、図２１Ａに示すように直線Lineに対して交差するか、図２１Ｂに示すように、例えばノイズなどの影響によりデータ点が一様に分布しているかを判別するものである。基本的には、直線Lineの一方にデータ点群Ｐ［0・・・n-1］が連続して現れる数をカウントし、ある一定数を超えて連続して現れる場合には、zig-zag-shapeであると判断することができる。図２１Ａの場合には、データ点群Ｐ［0・・・n-1］によりよくフィットする直線Lineを求めるためにデータ点群Ｐ［ｉ］を分割する必要がある。図２２は、Zig-Zag-Shape判別方法を示すフローチャートである。
まず、データ点群Ｐ［0・・・n-1］と直線Line（α,ｄ，σ）とを入力する（ステップＳ２０）。ここで、σは、点列の標準偏差を示す。次に、この標準偏差σが所定の閾値th_σより大きいか否かを判断する。この標準偏差σが閾値th_σより小さい場合（ステップＳ２１：Ｎｏ）は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、判別を終了する。そして、標準偏差σが閾値th_σより大きい場合のみ判別処理を継続する。次に、データ点群Ｐ［0・・・n-1］のうちの最初のデータ点Ｐ［0］が直線のどちら側にあるかをsing(sdist(P[0]))によって判断し、この結果をval0に代入するとともにval0と同じ側にあるデータ点の連続数をカウントするカウンタ（以下、連続点カウンタといい、このカウント値をカウント値countという。）のカウント値countを１に設定する（ステップＳ２２）。ここで、sign(x)は、ｘの値の符号（＋又は−）を返す関数であり、sdist(i)は、P[i].xcosα+ P[i].ycosα+dとして計算された直線Lineにおいて、ｉ番目のデータ点との正負の距離を示す。すなわち、Val0には、データ点Ｐ［0］が直線Lineのどちら側にあるかで＋又は−の符号が代入される。
次に、データ点をカウントするためのカウンタ（以下、データ点カウンタといい、このカウント値をカウント値ｉという。）のカウント値iを１とする（ステップＳ２３）。そして、データ点カウンタのカウント値ｉがデータ数ｎより小さい場合（ステップＳ２４：ＹＥＳ）、その次のデータ（以下、ｉ番目とする。）のデータ点であるデータ点Ｐ［ｉ］が直線のどちら側にあるかをsing(sdist(P[i]))によって判断し、この結果をvalに代入する（ステップＳ２５）。そして、ステップＳ２２にて求めたval0とステップＳ２５にて求めたvalとを比較し、val0とvalとが異なる場合（ステップＳ２６：ＮＯ）、val0にvalを代入し、連続点カウンタのカウント値countに１を代入し（ステップＳ２８）、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ３０）ステップＳ２４からの処理に戻る。
一方、ステップＳ２６において、val0とvalとが同じ場合（ステップＳ２６：ＹＥＳ）、データ点Ｐ［ｉ−１］とＰ［ｉ］は、直線Lineに対して同じ側にあると判断され、連続点カウンタのカウント値countを１つインクリメントする（ステップＳ２７）。さらに、連続点カウンタのカウント値countがZig-Zag-Shapeと判定されるための最小のデータ点数min_cより大きいか否か判定し（ステップＳ２９）、大きい場合には（ステップＳ２９：ＹＥＳ）、Zig-Zag-Shapeと判断し、TRUEを出力して処理を終了する。一方、連続点カウンタのカウント値countが最小のデータ点数min_cより小さい場合には（ステップＳ２９：ＮＯ）、ステップＳ３０に進み、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ３０）、ステップＳ２４からの処理を繰り返す。
そして、このステップＳ２４からの処理を、データ点カウンタのカウント値ｉがデータ点数ｎに到達するまで続け、カウント値ｉ≧ｎとなったところで、FALSEを出力して処理を終了する。
このようなジグザク形判別処理によって、ｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α,ｄ）：ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、このデータ点群が直線Lineに対してzig-zagに交差しているかどうかを判断することができる。これによって、上述したように、ステップＳ１６にてデータ点群を分割するべきかどうかを判断することができ、最小二乗により求めた直線に対し、データ点群がzig-zagに交差していると判断した場合にはデータ点群を分割すべきと判断してステップＳ１８の処理へ進み、着目点brkを分割点としてデータ点群を分割することができる。なお、上記ステップＳ２１〜ステップＳ３０までの処理は図２３のように表現することも可能である。
また、このようなZig-Zag-Shape判別処理は、演算器のみならずハードウェアで行うことも可能である。図２４は、Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。図２４に示すように、Zig-Zag-Shape判別処理部２０は、ｎ個のデータ点群Ｐ［0・・・n-1］が入力され、順次各データ点Ｐ[ｉ]が直線Lineのいずれ側に位置するかを判別し、その判別結果Valを出力する方向判別部２１と、１つ後のデータと方向判別部２１の結果を比較させるための遅延部２２と、データ点Ｐ[ｉ]における方向判別結果Valとデータ点Ｐ[ｉ−１]における方向判別結果Val0とを比較する比較部２３と、比較部２３においてVal＝Val0の場合に、カウント値をインクリメントする連続点カウンタ２４と、連続点カウンタ２４のカウント値countと最小データ点数格納部２６から読み出した最小データ点数min_cとを比較する比較部２５とを有する。
このZig-Zag-Shape判別処理部における動作は以下のようになる。すなわち、方向判別部２１は、データ点群Ｐ［0・・・n-1］から最小二乗法により直線Lineを求め、各データ点Ｐ[ｉ]と直線Lineとの正負の距離を求め、その正負の符号を出力する。遅延部２２は、データ点Ｐ[ｉ−１]の直線Lineまでの距離に対する正負の符号が入力されると１つ後のデータ点Ｐ[ｉ]の正負の符号が入力されるタイミングまでデータを格納する。
比較部２３は、データ点Ｐ[ｉ]とデータ点Ｐ[ｉ−１]の上記正負の符号を比較し、同じ符号である場合には接続点カウンタ２４のカウント値countをインクリメントする信号を出力し、正負の符号が異なればカウント値countに１を代入する信号を出力する。比較部２５は、カウント値countと最小データ点数min_cとを比較し、最小データ点数min_cよりカウント値countが大きい場合には、データ点群Ｐ［0・・・n-1］がジグザグであることを示す信号を出力する。
次に、図１０に示す領域拡張部（Region Growing）５について説明する。領域拡張部５は、線分抽出部４によって得られた線分群を入力とし、それらの線分それぞれがどの平面に属しているかを点列の平面への当てはめ（Plane Fitting）により判断し、与えられる線分群からなる領域を複数の平面（平面領域）に分離する。複数の平面に分離するために、以下の手法をとる。
先ず、与えられた線分群から、同じ平面上にあると推定される隣接する３本の線分を検索する。この３本の線分により求められる平面（基準平面）が、平面の種となるものであり、この３本の線分が含まれる領域を領域種（seed region）という。そして、この領域種に隣接する線分を順次、基準平面と同一平面上にある線分か否かを点列の平面への当てはめ（Plane Fitting）により判断し、隣接する線分が同じ平面に含まれると判断された場合には、この線分を領域拡大用の線分として領域種に追加してその領域を拡大するとともに、基準平面の方程式を上記領域拡大用の線分を含めて再度算出し直す。このような処理によって、全ての線分を何れかの領域（平面）に配分する。
図２５は、領域拡張処理を説明するための模式図である。図２５に示すように、画像３０内に複数の平面からなる階段３１が存在する場合、例えば太線で示す３２ａ〜３２ｃの３本の線分が領域種として選択されたとする。これら３本の線分３２ａ〜３２ｃからなる領域が領域種となる。先ず、この３つの線分３２ａ〜３２ｃにより１つの平面（基準平面）Ｐを求める。次に、領域種の最も外側の線分３２ａ又は３２ｃに領域種外にて隣接するそれぞれデータ列３３又は３４において、平面Ｐと同一の平面である線分を選択する。ここでは、線分３３ａが選択されるとする。次に、これら４本の線分群からなる平面Ｐ’を求め、基準平面Ｐを更新する。次に、線分３４ａが選択されれば、５本の線分群からなる平面Ｐ’’を求め、平面Ｐ’を更新する。これを繰り返すことにより、階段３１の２段目の踏面が、破線で囲まれる平面４５として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像３０内から領域種となる３つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる３つの線分がなくなるまで図１９のステップＳ３の処理を繰り返す。
次に、データ点群Ｐ［0・・・n-1］から構成される平面の方程式を推定する手法（Plane Fitting）、これを使用して領域種を選択する方法（Selection of seed region）、領域種から領域を拡大していく領域拡張処理（Region growing）、及び得られた平面方程式から誤差が大きいものなどを除いて再度算出する後処理（Post processing）について説明する。
３次元空間内の点ＰはＰ＝（ｘｉ，ｙｉ，ｚｉ）により表され、平面の方程式はその法線ベクトルｎ（ｎｘ，ｎｙ，ｎｚ）と非負の定数ｄによって下記式（４）で表される。
ここで、図１１に示すように、ステレオカメラでは、焦点を通る平面を観測することができない、すなわち、平面は焦点を通らないため、ｄ≠０とすることができる。したがって、平面は、最小二乗法により下記式（５）に示す値を最小にする値として求めることができる。
最適解はｎ＝ｍ／‖ｍ‖，ｄ＝−１／‖ｍ‖として求まる。ここで、‖・‖は、ベクトルの大きさ、ｍは、行列式によって連立一次方程式を解くクラメールの法則（Cramer’s rule）を使用して下記（６-１）のように容易に得られる線形システムの解である。
この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であっても、上記式（６−２）に示すＡとｂの値を更新するのみで、平面パラメータを再計算することができる。更に、本実施の形態における線分抽出方法の場合はｎ個のデータ点群の２つのモーメント（１次モーメント：平均、２次モーメント：分散）Ｅ（ｐ）、Ｅ（ｐｐＴ）が既知であり、これらを使用して、下記（７）に示すようにＡ，ｂを更新することができ、ｎ個のデータ点群における平面更新処理に拡張することができる。
また、一度平面パラメータｎ，ｄを算出すれば、求まった平面方程式から、ｎ個のデータ点群の平面方程式からの外れ度合いを示す平面方程式の２乗平均誤差（ＲＭＳ（root mean square）residual）（以下、ｒｍｓという。）を下記式（８）により算出することができる。この場合も、ｎ個のデータ点の上記２つのモーメントを使用して下記式（８）を求めることができる。
上記（８）に示すように、各データ点が求めた平面上にあれば平面方程式の２乗平均誤差ｒｍｓ（ｐ１・・・ｐｎ）は０になる値であり、この値が小さいほど各データ点が平面によくフィットしていることを示す。
次に、領域種（seed region）を検索する方法及び領域種から領域を拡大すると供に平面を更新する方法について説明する。図２６は、領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。図２６に示すように、領域種の選択には、先ず、線分抽出の際に使用した行方向又は列方向のデータ列が隣接する３つの線分（ｌ１，ｌ２，ｌ３）であって、互いの線分（ｌ１，ｌ２），（ｌ２，ｌ３）における画素位置が上記データ列とは直交する方向にて重複したものを検索する（ステップＳ３１）。各データ点は画像内における画素位置を示すインデックス（index）を有しており、例えば行方向のデータ列における線分である場合、このインデックスを比較して列方向にて重複しているか否かを比較する。この検索に成功した場合（ステップＳ３２：ＹＥＳ）、上記式（７）を使用して上記（６−１）を算出する。これにより、平面パラメータｎ，ｄを決定でき、これを使用して上記式（８）に示す平面方程式の２乗平均誤差（ｌ１，ｌ２，ｌ３）を計算する（ステップＳ３３）。そして、この平面方程式の２乗平均誤差ｒｍｓ（ｌ１，ｌ２，ｌ３）が例えば１ｃｍなどの所定の閾値ｔｈｒｍｓ１より小さい場合には、この３つの線分を領域種として選択する（ステップＳ３４）。所定の閾値ｔｈｒｍｓ１より大きい場合には、再びステップＳ３１に戻り、上記条件を満たす線分を検索する。また、領域種に選ばれた線分は、線分群のリストから除くことで、他の平面拡張などの際に使用されないようにしておく。
こうして選択された領域種から線分拡張法により領域を拡張する。すなわち、先ず、領域種の領域に追加する候補となる線分を検索する（ステップＳ３５）。なお、この領域は、領域種が既に更新されている場合の、後述する更新された領域種も含む。候補となる線分は、領域種の領域に含まれる線分（例えばｌ１）に隣接する線分（ｌ４）であって、上述同様、これらの線分の画素位置が相互に重なりあうことを条件とする。検索が成功した場合（ステップＳ３６：ＹＥＳ）、その平面方程式の２乗平均誤差ｒｍｓ（ｌ４）を算出し、これが所定の閾値ｔｈｒｍｓ２より小さいか否かを判定し（ステップＳ３７）、小さい場合には平面パラメータを更新し（ステップＳ３８）、再びステップＳ３５からの処理を繰り返す。ここで、候補となる線分がなくなるまで処理を繰り返し、候補となる線分がなくなったら（ステップＳ３６：ＮＯ）、ステップＳ３１の処理に戻り、再び領域種を検索する。そして、線分群に含まれる領域種がなくなった場合（ステップＳ３２：ＮＯ）、今まで得られている平面パラメータを出力して処理を終了する。
ここで、本実施の形態においては、領域種を検索し、３つの線分が同一平面に属するか否かの判定、及び領域拡張処理を行う際に基準平面又はこれを更新した更新平面に属するか否かの判定には、上記式（８）を使用する。すなわち、平面方程式の２乗平均誤差ｒｍｓが所定の閾値（ｔｈｒｍｓ１）未満である場合にのみその線分（群）を同一平面に属するものと推定し、その線分を含めた平面として再び平面を算出する。このように平面方程式の２乗平均誤差ｒｍｓを使用して同一平面に属するか否かを判定することにより、更にノイズにロバストでかつ、細かい段差を含んでいるような場合にも正確に平面を抽出することができる。以下にその理由について説明する。
図２７は、その効果を示す図であって、端点と直線との距離が等しくても平面方程式の２乗平均誤差ｒｍｓが異なる例を示す模式図である。ここで、非特許文献４のように、領域拡張処理する際、注目の直線（線分）の端点（end point）と平面Ｐとの距離Ｄの値が所定の閾値より小さい場合に、当該注目の線分が平面Ｐと同一平面であるとして領域拡張処理を行うと、平面Ｐに交差する直線Ｌａ（図２７Ａ）と、平面Ｐと平行で所定距離ずれているような直線Ｌｂ（図２７Ｂ）とが同様に平面Ｐの更新に使用されることとなる。ここで、平面方程式の２乗平均誤差ｒｍｓを求めると、図２７Ａの直線Ｌａから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌａ）に比して図２７Ｂの直線Ｌｂから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌｂ）の方が大きい。すなわち、図２７Ａのように、直線Ｌａと平面Ｐとが交差する場合は、平面方程式の２乗平均誤差ｒｍｓが比較的小さくノイズの影響である場合が多いのに対し、図２７Ｂのような場合、平面方程式の２乗平均誤差ｒｍｓが大きく、直線Ｌｂは平面Ｐと同一平面ではなく異なる平面Ｐ’である確率が高い。したがって、複数の平面が含まれるような環境から平面を精確に求める必要がある場合などにおいては、本実施の形態のように、平面方程式の２乗平均誤差ｒｍｓを算出し、この値が所定の閾値（ｔｈｒｍｓ２）未満である場合に同一平面と判断することが好ましい。
なお、環境や距離データの性質に応じて、従来と同様、線分の端点と平面との距離が所定の閾値以下の場合は当該線分を平面に含めるようにしたり、これらを組み合わせてもよい。
上記の領域拡張における線分追加検定において利用される閾値（ｔｈｒｍｓ２）は、実験等によって獲得された定数を用いることも可能だが、閾値の不適切な設定によるUnder-Segmentation, Over-Segmentationの問題が生じる為、線分に含まれる距離データの性質によって適応的に決定されることが望ましい。ノイズを多く含む線分群に対して低い閾値を設定すると、多くの線分が異なる領域に分割され、領域拡張が適切に行われない。
そこで本実施の形態では、下記式（９）により閾値（ｔｈｒｍｓ２）を線分に含まれるノイズに応じて変化させることで、線分データの精度に応じた適応的な判定を可能とした。
ｔｈｒｍｓ２＝max（ｔｈｒｍｓ３，ｄｍａｈａsigma(l２)） …（９）
ここで、ｔｈｒｍｓ３は閾値（ｔｈｒｍｓ２）の下限を定義する定数、ｄｍａｈａはマハラノビス距離、sigma()は線分の分散を表す。ノイズを多く含むデータは、線分の分散sigma()が大きく、閾値（ｔｈｒｍｓ２）が大きくなり、領域拡張に関する許容範囲が大きくなる。
ここでは、sigma()として上記式（２）で表される直線方程式とデータ点の誤差の総和Ｅｆｉｔを利用し、閾値の下限ｔｈｒｍｓ３は領域種の検定で利用した線分の許容誤差閾値ｔｈｒｍｓ１と同じとした。
また、面パラメータｎ，ｄを一旦算出すれば、平面方程式の２乗平均誤差ｒｍｓは、データ点群について線分抽出の際に求めた２つのモーメントの値から平面方程式を更新し、上記式（８）にて簡単に算出することができる。
また、上述の領域種の選択方法は、図２８のようにも表現することができる。overlap(lj，lk)は、各イメージロウに含まれる直線ベクトルｌｊとｌｋにおける端点間の位置が直線ベクトルとは直交する位置にて重なっている場合にtrueを出力する関数である。また、fitPlane(l1，l2，l3)は、上記式（４）〜（７）によりＡｍ＝ｂの解を求め平面パラメータｎ，ｄを計算し、上記式（８）により算出されたＡ，ｂにより、直線ベクトルｌ１，ｌ２，ｌ３を平面にフィッティングさせる関数である。
rms(l1，l2，l3)は、上記式（６）を使用して３本の直線全てにおいて、平面方程式の２乗平均誤差ｒｍｓの値を算出する関数である。また、remove(l1，l2，l3)は、lines［i］，lines［i+1］l2，lines［i+2］から領域種を構成するとして選択されたそれぞれ直線l1，l2，l3を除くことを意味し、これにより、再びこれらの直線が計算に使用されることを防止する。
また、領域拡張処理は、図２９のように表現することもできる。図２７において、Ａ及びｂは、上記式（６−１）に示すそれぞれ行列及びベクトルである、また、add(A，b，l)は、上記式（８）により、Ａとｂに直線lineのモーメントを加える関数である。Solve(A，b)は、Ａｍ＝ｂを満たすｍを求め、上記式（４）〜（７）により平面パラメータｎ，ｄを計算する。select(open)は、例えば最初の１つなど、openの中から任意に１つのエレメントを選択する関数である。また、index(l1)は、画素列又は行におけるl1のインデックスを返す関数である。また、neighbor(index)は、与えられたインデックスに隣接したインデックス、例えば{index-1，index+1}を返す関数である。
また、上述したように、本実施の形態においては、図１９のステップＳ３において領域拡張処理を行って平面方程式を更新した後、ステップＳ４において平面方程式を再度算出する処理（Post processing）を行う。この再度算出する処理では、例えば上述のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離データ点又は線分の平面からのずれを計算し、所定の値以上平面から外れる距離データ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低減することができる。
次に、このステップＳ４について詳細に説明する。ここでは、２つのステップにより、平面方程式を再度算出する方法について説明する。先ず、ステップＳ３にて検出された各平面の境界の距離データ点（pixels）において、現在属している平面よりも、隣接する平面までの距離が近いデータ点が検出された場合は、当該データ点を隣接する平面の方に含める処理をする。また、いずれの平面にも属していなく、且つ距離が例えば１．５ｃｍなど比較的大きい閾値以下である平面が存在するデータ点が検出できた場合は、当該データ点をその平面に含める処理をする。これらの処理は各平面領域の境界近傍のデータ点を検索することで実行することができる。以上の処理が終了したら、再度平面方程式を算出する。
次に、上述のようにして再度算出された平面の各領域の境界近傍において、各データ点と平面との距離が例えば０．７５ｃｍなど比較的小さい閾値を超える場合は、それらのデータ点を捨てる処理を実行する。これにより、その平面領域は若干小さくなるものの更に精確な平面を求めることができる。距離データ点を削除後、再び平面を求め、この処理を繰り返す。このことにより、極めて精密に平面を求めることができる。
次に各処理によって得られる結果を示す。図３０Ａは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図３０Ｂは、縦軸をｘ、横軸をｙ、各データ点の濃淡でｚ軸を表現して３次元距離データを示す図であり、更に、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す。図３０Ｂに示す直線群から領域拡張処理によりえられた平面領域を図３０Ｃに示す。このように、ロボット装置の視野内には、１つの平面（床面）のみが存在する、すなわち、床面が全て同じ平面として検出されていることがわかる。
次に、床面に段差を一段置いたときの結果を図３１に示す。図３１Ａに示すように、床面Ｆには、１段の段差ＳＴ３が載置されている。図３１Ｂは、実験条件を示す図であり、着目点と直線（線分）との距離がmax_dを超える場合は、データ点群を分割する。また、抽出の成否（水平）（correct extraction(horizontal)）は、行方向のデータ列毎に、合計１０回の線分抽出を行う線分拡法による平面検出を行って成功した回数を示し、抽出の成否（垂直）（correct extraction(vertical)）は、列方向のデータ列毎についての抽出の成否を示す。また、Ｎｏ．１〜Ｎｏ．５は、上述したZig-Zag-Shape判別処理を取り入れていない従来の線分拡張法による平面検出処理の条件、Ｎｏ．６は、Zig-Zag-Shape判別処理を行った本実施の形態における平面検出方法の条件を示す。
図３１Ｃ及び図３１Ｄは、線分拡張法により平面検出した結果を示す図であって、それぞれ本実施の形態における手法により平面検出した結果、従来の線分拡張法により平面検出した結果（比較例）を示す。図３１Ｂに示すように、従来の手法においては、線分抽出（Line Fitting）において推定のための閾値パラメータmax_dを大きくする（max_d＝２５，３０）と検出率が下がり、閾値max_d小さくする(max_d＝１０，１５)と検出率が向上する。これに対して、本発明のように、ジグザグ形検証処理を導入することにより、大きな閾値max_d＝３０を設定しても、優れた検出結果を示すことがわかる。
すなわち、閾値max_dを大きくすると、ノイズの影響が少なくなるものの、線分抽出が難しくなり、閾値max_dを小さくすると、ノイズの影響を受けて誤検出が多くなってしまう。図３２Ａに示す床面を撮影した画像から３次元距離データを取得した場合を図３２Ｂ及び図３２Ｃに示す。いずれ左図は、行方向の画素列（距離データ列）から線分を抽出した例、右図は列方向の画素列（距離データ列）から線分を抽出した例を示す。図３２Ｂに示すように、閾値max_dを小さくすると、ノイズの影響が大きくなり、ノイズの影響が大きい遠方などにおいては特に、線分をうまく検出することができない。一方、図３２Ｃに示すように、従来の線分抽出に更にジグザグ形判別処理を加えた場合、閾値max_dを大きくしても、更にノイズの影響が大きい遠方の領域であっても線分が検出されていることがわかる。
図３３及び図３４は、それぞれ異なる階段を撮影した画像から３次元距離データを取得して平面検出した例を示す図である。図３３及び図３４に示すように、いずれの場合も全ての踏面を平面として検出できている。また、図３４Ｂは、床面の一部も他の平面として検出成功していることを示す。
本実施の形態によれば、線分拡張法による平面検出を行う際、始めは大きな閾値を設定して線分を分割し、次にZig-Zag-Shape判別処理により、閾値を超えるデータ点を持たない直線であってもジグザグ形である場合には、ノイズではなく、複数平面からなる直線であるとして線分を分割するようにしたので、ノイズを含む距離情報から複数の平面を精度よく検出することが可能となる。
このように、小さい段差も精度よく検出することができるため、例えばロボット装置が移動可能な環境内の階段などを認識することができ、二足歩行ロボット装置であれば、この検出結果を利用して階段昇降動作が可能となる。
更に、複数の平面によって構成されている凸凹の床面を歩行可能な平面だと誤認識することがなくなり、ロボット装置の移動などが更に簡単になる。
なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。また、上述した線分抽出処理、ジグザグ形検証処理、領域拡大処理などの各処理のうち１以上の任意の処理は、ハードウェアで構成しても、演算器（ＣＰＵ）にコンピュータプログラムを実行させることで実現してもよい。コンピュータプログラムとする場合には、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。
本実施の形態における平面検出装置は、ステレオビジョンなどにより得られた距離情報（距離データ）から複数の平面を抽出することができ、計測ノイズに対してロバストな検出を行うことが可能であり、例えば、ロボット装置に搭載すれば、ロボット装置が自身の周囲の環境を正確に認識することができ、例えば階段などの複数平面を含む物体、周囲の状況を認識し、この認識結果に応じて自律的に移動したり、行動したりすることができる。
本実施の形態においては、まず、このようなロボット装置の一例として２足歩行タイプのロボット装置を例にとって説明する。このロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボット装置である。なお、ここでは、２足歩行型のロボット装置を例にとって説明するが、２足歩行のロボット装置に限らず、４足又は車輪等により移動可能なロボット装置に適用できることはいうまでもない。
図７は、本実施の形態におけるロボット装置の概観を示す斜視図である。図７に示すように、ロボット装置２０１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。
このロボット装置２０１が具備する関節自由度構成を図８に模式的に示す。頭部ユニット２０３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
また、上肢を構成する各々の腕部ユニット２０４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール輪１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置２０１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
また、体幹部ユニット２０２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
また、下肢を構成する各々の脚部ユニット２０５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足底１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置２０１の股関節位置を定義する。人体の足底１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置２０１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
以上を総括すれば、ロボット装置２０１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置２０１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
上述したようなロボット装置２０１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うこと等の要請から、アクチュエータは小型且つ軽量であることが好ましい。
このようなロボット装置は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット２０２等に備える。図９は、ロボット装置２０１の制御システム構成を示す模式図である。図９に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール２００と、アクチュエータ３５０の駆動等、ロボット装置２０１の全身協調運動を制御する運動制御モジュール３００とで構成される。
思考制御モジュール２００は、情緒判断や感情表現に関する演算処理を実行するＣＰＵ（Central Processing Unit）２１１や、ＲＡＭ（Random Access Memory）２１２、ＲＯＭ（Read Only Memory）２１３及び外部記憶装置（ハード・ディスク・ドライブ等）２１４等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。
この思考制御モジュール２００は、画像入力装置２５１から入力される画像データや音声入力装置２５２から入力される音声データ等、外界からの刺激等に従って、ロボット装置２０１の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット装置２０１の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置２５１は、例えばＣＣＤ（Charge Coupled Device）カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。また、音声入力装置２５２は、例えばマイクロホンを複数備えている。
思考制御モジュール２００は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール３００に対して指令を発行する。
一方の運動制御モジュール３００は、ロボット装置２０１の全身協調運動を制御するＣＰＵ３１１や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。
この運動制御モジュール３００には、図８に示したロボット装置２０１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インタフェース（Ｉ／Ｆ）３１０経由で接続されている。ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組み合わせによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。
思考制御モジュール２００と運動制御モジュール３００は、共通のプラットフォーム上で構築され、両者間はバス・インタフェース２１０，３１０を介して相互接続されている。
運動制御モジュール３００では、思考制御モジュール２００から指示された行動を体現すべく、各アクチュエータ３５０による全身協調運動を制御する。すなわち、ＣＰＵ３１１は、思考制御モジュール２００から指示された行動に応じた動作パターンを外部記憶装置３１４から取り出し、又は、内部的に動作パターンを生成する。そして、ＣＰＵ３１１は、指定された動作パターンに従って、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各アクチュエータ３５０に転送する。
また、ＣＰＵ３１１は、姿勢センサ３５１の出力信号によりロボット装置２０１の体幹部ユニット２０２の姿勢や傾きを検出するとともに、各接地確認センサ３５２，３５３の出力信号により各脚部ユニット２０５Ｒ／Ｌが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット装置２０１の全身協調運動を適応的に制御することができる。更に、ＣＰＵ３１１は、ＺＭＰ位置が常にＺＭＰ安定領域の中心に向かうように、ロボット装置２０１の姿勢や動作を制御する。
また、運動制御モジュール３００は、思考制御モジュール２００において決定された意思通りの行動がどの程度発現されたか、すなわち処理の状況を、思考制御モジュール２００に返すようになっている。このようにしてロボット装置２０１は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
このようなロボット装置においては、頭部ユニット２０３にステレオビジョンシステムを搭載し、外界の３次元距離情報を取得することができる。次に、このようなロボット装置などに好適に搭載されるものであって、ステレオビジョンによる３次元距離情報を利用した本実施の形態における平面検出装置について説明する。なお、距離情報としては、レーザレンジファインダ（レーザ距離計測計）などによる距離情報を使用してもよいことは勿論である。
本実施の形態における平面検出装置は、線分拡張法により、視野内において支配的な平面だけでなく、例えば階段など複数の平面が存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさせることにより計測ノイズに対してロバストな平面検出結果を得ることができるものである。
図１０は、本実施の形態における平面検出装置を示す機能ブロック図である。図１０に示すように、平面検出装置１は、３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）２と、３次元の距離データからなる距離画像に存在する平面を線分拡張法により検出する平面検出部３とを有する。平面検出部３は、画像を構成する距離データ点から同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部４と、画像内に含まれる、線分抽出部４よって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部５とを有する。領域拡張部５は、線分群から同一平面上に存在すると推定される任意の３本の線分を選択し、これらから基準平面を求める。そして、選択した３本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新するとともに基準平面の領域を拡張する。
ステレオビジョンシステム２は、例えばロボット装置２０１の画像入力装置２５１により取得された画像から距離画像を生成するものであり、外界を観測した結果、両眼の視差によって推定される３次元距離データＤ１を線分抽出部４に出力する。
線分抽出部４は、その距離画像における列または行毎の各データ列において、３次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて１以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群Ｄ２を領域拡張部５に出力する。
領域拡張部５は、この線分群Ｄ２において、同一の平面に属すると推定される線分を３本選択し、これらから基準平面としての種となる平面を求める。この種となる平面の領域（領域種：seed region）に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群Ｄ３を出力する。
ロボット装置２０１は、障害物回避や階段昇降など平面の情報が必要なとき、または定期的にこれらの処理を行うことによって、階段や床面、壁といった歩行に重要な平面の情報を取得する。
次に、平面検出装置１を構成するステレオビジョンシステム２について更に詳細に説明する。ステレオビジョンシステム２は、人間の両眼に相当する左右２つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。
図１１は、ロボット装置２０１が外界を撮影している様子を示す模式図である。床面をｘ−ｙ平面とし、高さ方向をｚ方向としたとき、図１１に示すように、画像入力部（ステレオカメラ）を頭部ユニット２０３に有するロボット装置２０１の視野範囲は、ロボット装置２０１の前方の所定範囲となる。
ロボット装置２０１は、上述したＣＰＵ２１１において、画像入力装置２５１からのカラー画像及び視差画像と、各アクチュエータ３５０の全ての関節角度等のセンサデータとなどが入力されてソフトウェア構成を実現する。
本実施の形態のロボット装置２０１におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。なお、ロボット装置の位置を示す座標として、例えば、ランドマーク等の特定の物体等に基づく所定位置を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心（座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との２つの座標を使用する。
ステレオビジョンシステム２では、カラー画像及びステレオカメラによる視差画像などの画像データが撮像された時間において、センサデータから割り出した関節角を使用してロボット装置２０１が中心に固定されたロボット中心座標系を頭部ユニット２０３に設けられた画像入力装置２５１の座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列とこれに対応する３次元距離データからなる距離画像を平面検出部３へ出力する。
ここで、本実施の形態における平面検出装置は、上述したハフ変換のように、取得した画像に含まれる支配的な平面を検出するのみならず、階段など複数平面が含まれている場合であっても平面を検出可能とするため、線分拡張法により平面を検出する。そして、この際、距離データ点の分布に応じて線分を生成することにより、計測ノイズに対してロバストな検出結果を得ることができるものである。以下では、本実施の形態における平面検出装置を搭載したロボット装置が視野内に含まれる階段ＳＴを検出する場合を例にとって説明する。
階段としては、例えば図１２、図１３に示すようなものがある。図１２Ａ、図１３Ａは、階段を正面から見た図、図１２Ｂ）、図１３Ｂは、階段を側面から見た図、図１２Ｃ、図１３Ｃは、階段を斜めから見た図である。
ここで、本明細書においては、人間、ロボット装置などが階段を昇降するために使用する面（足又は可動脚部を載せる面）を踏面といい、一の踏面からその次の踏面までの高さ（１段の階段の高さ）を蹴り上げということとする。また、階段は、地面に近い方から１段目、２段目とカウントすることとする。
図１２に示す階段ＳＴ１は、段数が３段の階段であり、蹴り上げ４ｃｍ、１，２段面の踏面の大きさは幅３０ｃｍ、奥行き１０ｃｍ、最上段である３段目の踏面のみ、幅３０ｃｍ、奥行き２１ｃｍとなっている。また、図１３に示す階段ＳＴ２も段数が３段の階段であり、蹴り上げ３ｃｍ、１，２段面の踏面の大きさは幅３３ｃｍ、奥行き１２ｃｍ、最上段である３段目の踏面のみ、幅３３ｃｍ、奥行き３２ｃｍとなっている。
図１４乃至図１６は、図１３に示す階段ＳＴ２を示し、図１４Ａ、図１５Ａ、図１６Ａは、図１３に示す階段をステレオビジョンシステムによってそれぞれ前方、側方、斜め前方から撮影した場合の画像を示す模式図、図１４Ｂ乃至図１６Ｄは、図１４Ａ、図１５Ａ、図１６Ａに示す画像から取得した３次元の距離データを示す図である。
図１４Ａに示すように、正面から階段ＳＴ２を撮影した場合、３次元の距離データは、図１４Ｂ乃至図１４Ｄのようになる。図１４Ｂにおいて、横軸をｙ方向、縦軸をｘ方向とし、ｚ軸方向（高さ方向）の大きさは、ロボット装置２０１の接地面を０とし、高さが高くなるほど白に近づくような濃淡値で示している。すなわち、濃淡（濃淡値）が同様のデータ点は同一高さにあることを示し、図１４Ｂに示すように、階段ＳＴ２においても１段目より２段目、２段目より３段目の踏面にあたる領域のデータ点の濃淡が薄くなっている。また、距離データが図示されている略台形領域がロボット装置が撮影可能な範囲（視野範囲）を示す。また、同図において、距離データ点は略４段階の濃淡に分かれているが、ｚ方向が最も小さい領域に対応する最も濃淡が濃い箇所は、床面を示している。また、図１４Ｃは、横軸をｙ方向、縦軸をｚ方向、ｘ方向を色の濃淡で示すものである。この図では、ｘ方向の距離が大きくなるほど濃淡が薄くなるように表現されている。図１４Ｄは、横軸をｘ方向、縦軸をｚ方向とし、ｙ方向をその距離に応じて濃淡で表現している。
また、ロボット装置２０１が階段ＳＴ２の側面を撮影した場合は、図１５Ａ乃至図１５Ｄに示すように、ｘ軸が大きい上方側の領域に存在するデータ点が、高さが０と同様の濃淡を示しており、階段ＳＴ２の奥の床面を計測した結果であることを示している。また、図１６Ａ乃至図１６Ｄに示す斜め方向からの撮像においても、床面及び１〜３段目の踏面を示す４つの領域が、高さの違いに応じて異なる濃淡で示され、はっきり区別できていることを示している。
ここで、このような３次元距離データをステレオカメラによって取得するためには、階段ＳＴ２の表面に模様（テクスチャ）が必要となる。すなわち、２台のカメラによる視差により得ることができるため、模様がないものは視差が算出できず、正確に距離を計測することができない。すなわち、ステレオビジョンシステムにおける距離データの計測精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
そこで、図１７に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステムを構成するステレオカメラ１１Ｒ／Ｌを備えるとともに、例えば同じく頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源１２を設ける。この光源１２は、模様がない階段ＳＴ３、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射（照射）し、ランダムなパターンＰＴを付与する模様付与手段として作用する。なお、ランダムパターンＰＴを形成して距離画像を取得できるものであれば、ランダムパターンＰＴを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載されるＣＣＤカメラなどにおいては観測可能なパターンを付与することができる。
次に、平面検出装置１の平面検出部３について説明する。この平面検出部３は、線分拡張法を使用して平面を検出するものであり、図１８は、線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出では、図１８に示すように、まず、焦点Ｆから撮影された画像１１において、行方向または列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row：イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分からなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。
図１９は、線分拡張法による平面検出処理を示すフローチャートである。図１９に示すように、先ず、距離画像を入力し（ステップＳ１）、距離画像の行方向（又は列方向）の各画素列において同一平面に属すると推定されるデータ点から線分を求める（ステップＳ２）。そして、これらの線分群の中から同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める（ステップＳ３）。このステップＳ３では、まず、平面の種となる領域（以下、領域種（seed region）という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の１ラインを含む３本の線分が同一平面にあることを条件とする。ここで、選択した３本の線分からなる領域種が属する平面を基準平面とし、３本の線分から平均して求まる平面を求めておく。また、３本の線分からなる領域を基準平面領域とする。
そして、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し（領域拡張処理）、追加した線分を含めたものとして上記基準平面を更新し（平面更新処理）、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域（３本の線分）が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結する。そして、本実施の形態においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップＳ４として更に設け、最終的な平面とするが、詳細は後述する。
ここで、３次元距離データから線分を検出し、これを同一平面毎にまとめた領域を１つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本実施の形態においては、ステップＳ２における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しようとしても、距離データの精度に応じて閾値を変更しなければ、over-segmentation又はunder-segmentationなどもの問題が生じてしまう。そこで、本実施の形態においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。
以下、図１９に示す線分拡張法による平面検出方法について更に詳細に説明する。線分抽出器(Line Extraction)４は、上述したように、ステレオビジョンシステム２からの３次元距離画像を入力とし、距離画像の各列または各行毎に３次元空間内で同一平面上にあると推定される線分を検出する。この線分抽出において、計測ノイズなどによる、上述したover-segmentationやunder-segmentationの問題、すなわち、本来は複数の平面であるのに１つの平面として認識してしまったり、本来は１つの平面であるのに、複数の平面として認識してしまったりする問題を回避するため、データ点の分布に応じて適応的に線分フィッティングさせるアルゴリズム（Adaptive Line Fitting）を導入する。Adaptive Line Fittingは、線分抽出部４において、先ず比較的大きい閾値を使用して大まかに第１の線分としての線分を抽出し、次に抽出された第１の線分に属するデータ点群から後述する最小二乗法によって得られる第２の線分としての線分に対する該データ点群の分布を解析する。すなわち、同一平面上に存在するか否かを大まかに推定してデータ点群を抽出し、抽出したデータ点群におけるデータ点の分布の偏りがあるか否かを解析して同一平面上に存在しているか否かを再度推定する。
本実施の形態においては、このデータ点の分布を解析し、データ点群が後述するジグザグ形（zig-zag-shape）に当てはまる場合には、分布に偏りがあるとしてデータ点群を分割する処理を行い、これを繰り返すことによって、データ点群に含まれるノイズに対して適応的に線分の抽出を行うアルゴリズムを使用するものとする。
図２０は、線分抽出部４における処理、すなわち、図１９におけるステップＳ２の処理の詳細を示すフローチャートである。まず、線分抽出部４には、距離データが入力される。入力された距離データのうち、例えば行方向の画素列（データ点列）において、３次元空間上で同一平面上に存在すると推定されるデータ点群を抽出する。３次元空間上で同一平面上に存在すると推定されるデータ点群は、例えば隣接するデータ点の距離が、例えば６ｃｍ以下など、データ点間の３次元空間における距離が所定の閾値以下のものからなるデータ点の集合などとすることができ、これをデータ点群（Ｐ［0・・・n-1］）として抽出する（ステップＳ１１）。そして、このデータ点群Ｐ［0・・・n-1］に含まれるサンプル数ｎが処理に最低限必要なサンプル数（必要最小値）min_nより多いか否かをチェックし（ステップＳ１２）、データ数ｎが必要最小値min_nより少ない場合（Ｓ２：ＹＥＳ）には、検出結果として空集合を出力して処理を終了する。
一方、サンプル数ｎが必要最小値min_n以上である場合（Ｓ２：ＮＯ）、データ点群Ｐ［0・・・n-1］の一方の端点Ｐ［0］と他方の端点Ｐ［n-1］とを結ぶ線分（弦）Ｌ１を第１の線分として生成する。そして、データ点群Ｐ［0・・・n-1］から、この線分Ｌ１との距離が最も大きいデータ点を着目点brkとして検索し、その距離distを算出する（ステップＳ１３）。最大距離distがデータ点群分割の閾値max_dより大きい場合には（Ｓ１４：ＹＥＳ）、データ点群データ点群Ｐ［0・・・n-1］を着目点（分割点）brkにて２つのデータ点群ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する（ステップＳ１８）。
一方、最大距離distがデータ点群分割の閾値max_dより小さい場合には（Ｓ１４：ＮＯ）、データ点群Ｐ［0・・・n-1］から後述する最小二乗法によって最適な線分の方程式lineを求め（ステップＳ１５）、この方程式lineが示す線分Ｌ２を第２の線分として生成する。そして、データ点群Ｐ［0・・・n-1］がこの線分Ｌ２に対して後述するZig-Zag-Shapeであるかどうかを調べる（ステップＳ１６）、Zig-Zag-Shapeでない場合（S１６：ＮＯ）、得られた線分の方程式lineを線分抽出結果リストに追加し（ステップＳ１７）、処理を終了する。
また、ステップＳ１６においてステップＳ１５で求めた線分がZig-Zag-Shapeである判断された場合（Ｓ１６：ＹＥＳ）の場合、上述のステップＳ１４と同様、ステップＳ１８に進み、ステップＳ１３において距離distを求めた着目点brkにてデータ点群を２つのデータ点群Ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する。このステップＳ１８にて２つのデータ点群が得られた場合には、それぞれを再帰的に再度ステップＳ１１からの処理を行う。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すなわち全てのデータ点群がステップＳ１７を経るまで処理を繰り返し、これにより、全ての線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群Ｐ［0・・・n-1］からノイズの影響を排除し複数の線分からなる線分群を精度よく検出することができる。
なお、ステップＳ１３にてデータ点群Ｐ［0・・・n-1］の端点を結ぶ線分Ｌ１を生成するものとして説明したが、例えばデータ点群Ｐ［0・・・n-1］の分布、性質など必要に応じてデータ点群Ｐ［0・・・n-1］から最小二乗により線分Ｌ１を求めてもよい。また、本実施の形態においては、着目点brkは、端点を結んだ線分Ｌ１との距離が最大の点１つとしているが、例えば、上記のように最小二乗により求めた線分との距離が最大の点としたり、距離がデータ点群分割の閾値max_d以上のものが複数ある場合はそれら全ての点又は選択した１つ以上にてデータ点群Ｐ［0・・・n-1］を分割するようにしてもよい。
次に、ステップＳ１５における最小二乗による線分生成方法（Least-Squares Line Fitting）について説明する。あるｎ個のデータ点群Ｐ［0・・・n-1］が与えられたとき、データ点群に最もフィットした直線の方程式を求める方法を示す。直線の方程式のモデルを下記式（１）で表す。
この場合、ｎ個のデータ点群Ｐ［0・・・n-1］の１点（ｘｉ,ｙｉ）において、直線方程式のモデルとデータ点との誤差の総和は下記式（２）で表すことができる。
データ点群に最もフィットした直線は、上記式（２）の誤差の総和を最小化することによって求められる。上記式（２）を最小にするα及びｄは、データ点群Ｐの平均及び分散共分散行列を用いて下記（３）のように求めることができる。
次に、ステップＳ１６におけるジグザグ形（Zig-Zag-Shape）判別方法について説明する。このZig-Zag-Shape判別では、あるｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α，ｄ）、ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、そのデータ点群Ｐ［0・・・n-1］が、図２１Ａに示すように直線Lineに対して交差するか、図２１Ｂに示すように、例えばノイズなどの影響によりデータ点が一様に分布しているかを判別するものである。基本的には、直線Lineの一方にデータ点群Ｐ［0・・・n-1］が連続して現れる数をカウントし、ある一定数を超えて連続して現れる場合には、zig-zag-shapeであると判断することができる。図２１Ａの場合には、データ点群Ｐ［0・・・n-1］によりよくフィットする直線Lineを求めるためにデータ点群Ｐ［ｉ］を分割する必要がある。図２２は、Zig-Zag-Shape判別方法を示すフローチャートである。
まず、データ点群Ｐ［0・・・n-1］と直線Line（α,ｄ，σ）とを入力する（ステップＳ２０）。ここで、σは、点列の標準偏差を示す。次に、この標準偏差σが所定の閾値th_σより大きいか否かを判断する。この標準偏差σが閾値th_σより小さい場合（ステップＳ２１：Ｎｏ）は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、判別を終了する。そして、標準偏差σが閾値th_σより大きい場合のみ判別処理を継続する。次に、データ点群Ｐ［0・・・n-1］のうちの最初のデータ点Ｐ［0］が直線のどちら側にあるかをsing(sdist(P[0]))によって判断し、この結果をval0に代入するとともにval0と同じ側にあるデータ点の連続数をカウントするカウンタ（以下、連続点カウンタといい、このカウント値をカウント値countという。）のカウント値countを１に設定する（ステップＳ２２）。ここで、sign(x)は、ｘの値の符号（＋又は−）を返す関数であり、sdist(i)は、P[i].xcosα+ P[i].ycosα+dとして計算された直線Lineにおいて、ｉ番目のデータ点との正負の距離を示す。すなわち、Val0には、データ点Ｐ［0］が直線Lineのどちら側にあるかで＋又は−の符号が代入される。
次に、データ点をカウントするためのカウンタ（以下、データ点カウンタといい、このカウント値をカウント値ｉという。）のカウント値iを１とする（ステップＳ２３）。そして、データ点カウンタのカウント値ｉがデータ数ｎより小さい場合（ステップＳ２４：ＹＥＳ）、その次のデータ（以下、ｉ番目とする。）のデータ点であるデータ点Ｐ［ｉ］が直線のどちら側にあるかをsing(sdist(P[i]))によって判断し、この結果をvalに代入する（ステップＳ２５）。そして、ステップＳ２２にて求めたval0とステップＳ２５にて求めたvalとを比較し、val0とvalとが異なる場合（ステップＳ２６：ＮＯ）、val0にvalを代入し、連続点カウンタのカウント値countに１を代入し（ステップＳ２８）、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ３０）ステップＳ２４からの処理に戻る。
一方、ステップＳ２６において、val0とvalとが同じ場合（ステップＳ２６：ＹＥＳ）、データ点Ｐ［ｉ−１］とＰ［ｉ］は、直線Lineに対して同じ側にあると判断され、連続点カウンタのカウント値countを１つインクリメントする（ステップＳ２７）。さらに、連続点カウンタのカウント値countがZig-Zag-Shapeと判定されるための最小のデータ点数min_cより大きいか否か判定し（ステップＳ２９）、大きい場合には（ステップＳ２９：ＹＥＳ）、Zig-Zag-Shapeと判断し、TRUEを出力して処理を終了する。一方、連続点カウンタのカウント値countが最小のデータ点数min_cより小さい場合には（ステップＳ２９：ＮＯ）、ステップＳ３０に進み、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ３０）、ステップＳ２４からの処理を繰り返す。
そして、このステップＳ２４からの処理を、データ点カウンタのカウント値ｉがデータ点数ｎに到達するまで続け、カウント値ｉ≧ｎとなったところで、FALSEを出力して処理を終了する。
このようなジグザク形判別処理によって、ｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α,ｄ）：ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、このデータ点群が直線Lineに対してzig-zagに交差しているかどうかを判断することができる。これによって、上述したように、ステップＳ１６にてデータ点群を分割するべきかどうかを判断することができ、最小二乗により求めた直線に対し、データ点群がzig-zagに交差していると判断した場合にはデータ点群を分割すべきと判断してステップＳ１８の処理へ進み、着目点brkを分割点としてデータ点群を分割することができる。なお、上記ステップＳ２１〜ステップＳ３０までの処理は図２３のように表現することも可能である。
また、このようなZig-Zag-Shape判別処理は、演算器のみならずハードウェアで行うことも可能である。図２４は、Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。図２４に示すように、Zig-Zag-Shape判別処理部２０は、ｎ個のデータ点群Ｐ［0・・・n-1］が入力され、順次各データ点Ｐ[ｉ]が直線Lineのいずれ側に位置するかを判別し、その判別結果Valを出力する方向判別部２１と、１つ後のデータと方向判別部２１の結果を比較させるための遅延部２２と、データ点Ｐ[ｉ]における方向判別結果Valとデータ点Ｐ[ｉ−１]における方向判別結果Val0とを比較する比較部２３と、比較部２３においてVal＝Val0の場合に、カウント値をインクリメントする連続点カウンタ２４と、連続点カウンタ２４のカウント値countと最小データ点数格納部２６から読み出した最小データ点数min_cとを比較する比較部２５とを有する。
このZig-Zag-Shape判別処理部における動作は以下のようになる。すなわち、方向判別部２１は、データ点群Ｐ［0・・・n-1］から最小二乗法により直線Lineを求め、各データ点Ｐ[ｉ]と直線Lineとの正負の距離を求め、その正負の符号を出力する。遅延部２２は、データ点Ｐ[ｉ−１]の直線Lineまでの距離に対する正負の符号が入力されると１つ後のデータ点Ｐ[ｉ]の正負の符号が入力されるタイミングまでデータを格納する。
比較部２３は、データ点Ｐ[ｉ]とデータ点Ｐ[ｉ−１]の上記正負の符号を比較し、同じ符号である場合には接続点カウンタ２４のカウント値countをインクリメントする信号を出力し、正負の符号が異なればカウント値countに１を代入する信号を出力する。比較部２５は、カウント値countと最小データ点数min_cとを比較し、最小データ点数min_cよりカウント値countが大きい場合には、データ点群Ｐ［0・・・n-1］がジグザグであることを示す信号を出力する。
次に、図１０に示す領域拡張部（Region Growing）５について説明する。領域拡張部５は、線分抽出部４によって得られた線分群を入力とし、それらの線分それぞれがどの平面に属しているかを点列の平面への当てはめ（Plane Fitting）により判断し、与えられる線分群からなる領域を複数の平面（平面領域）に分離する。複数の平面に分離するために、以下の手法をとる。
先ず、与えられた線分群から、同じ平面上にあると推定される隣接する３本の線分を検索する。この３本の線分により求められる平面（基準平面）が、平面の種となるものであり、この３本の線分が含まれる領域を領域種（seed region）という。そして、この領域種に隣接する線分を順次、基準平面と同一平面上にある線分か否かを点列の平面への当てはめ（Plane Fitting）により判断し、隣接する線分が同じ平面に含まれると判断された場合には、この線分を領域拡大用の線分として領域種に追加してその領域を拡大するとともに、基準平面の方程式を上記領域拡大用の線分を含めて再度算出し直す。このような処理によって、全ての線分を何れかの領域（平面）に配分する。
図２５は、領域拡張処理を説明するための模式図である。図２５に示すように、画像３０内に複数の平面からなる階段３１が存在する場合、例えば太線で示す３２ａ〜３２ｃの３本の線分が領域種として選択されたとする。これら３本の線分３２ａ〜３２ｃからなる領域が領域種となる。先ず、この３つの線分３２ａ〜３２ｃにより１つの平面（基準平面）Ｐを求める。次に、領域種の最も外側の線分３２ａ又は３２ｃに領域種外にて隣接するそれぞれデータ列３３又は３４において、平面Ｐと同一の平面である線分を選択する。ここでは、線分３３ａが選択されるとする。次に、これら４本の線分群からなる平面Ｐ’を求め、基準平面Ｐを更新する。次に、線分３４ａが選択されれば、５本の線分群からなる平面Ｐ’’を求め、平面Ｐ’を更新する。これを繰り返すことにより、階段３１の２段目の踏面が、破線で囲まれる平面４５として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像３０内から領域種となる３つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる３つの線分がなくなるまで図１９のステップＳ３の処理を繰り返す。
次に、データ点群Ｐ［0・・・n-1］から構成される平面の方程式を推定する手法（Plane Fitting）、これを使用して領域種を選択する方法（Selection of seed region）、領域種から領域を拡大していく領域拡張処理（Region growing）、及び得られた平面方程式から誤差が大きいものなどを除いて再度算出する後処理（Post processing）について説明する。
３次元空間内の点ＰはＰ＝（ｘｉ，ｙｉ，ｚｉ）により表され、平面の方程式はその法線ベクトルｎ（ｎｘ，ｎｙ，ｎｚ）と非負の定数ｄによって下記式（４）で表される。
ここで、図１１に示すように、ステレオカメラでは、焦点を通る平面を観測することができない、すなわち、平面は焦点を通らないため、ｄ≠０とすることができる。したがって、平面は、最小二乗法により下記式（５）に示す値を最小にする値として求めることができる。
最適解はｎ＝ｍ／‖ｍ‖，ｄ＝−１／‖ｍ‖として求まる。ここで、‖・‖は、ベクトルの大きさ、ｍは、行列式によって連立一次方程式を解くクラメールの法則（Cramer’s rule）を使用して下記（６-１）のように容易に得られる線形システムの解である。
この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であっても、上記式（６−２）に示すＡとｂの値を更新するのみで、平面パラメータを再計算することができる。更に、本実施の形態における線分抽出方法の場合はｎ個のデータ点群の２つのモーメント（１次モーメント：平均、２次モーメント：分散）Ｅ（ｐ）、Ｅ（ｐｐＴ）が既知であり、これらを使用して、下記（７）に示すようにＡ，ｂを更新することができ、ｎ個のデータ点群における平面更新処理に拡張することができる。
また、一度平面パラメータｎ，ｄを算出すれば、求まった平面方程式から、ｎ個のデータ点群の平面方程式からの外れ度合いを示す平面方程式の２乗平均誤差（ＲＭＳ（root mean square）residual）（以下、ｒｍｓという。）を下記式（８）により算出することができる。この場合も、ｎ個のデータ点の上記２つのモーメントを使用して下記式（８）を求めることができる。
上記（８）に示すように、各データ点が求めた平面上にあれば平面方程式の２乗平均誤差ｒｍｓ（ｐ１・・・ｐｎ）は０になる値であり、この値が小さいほど各データ点が平面によくフィットしていることを示す。
次に、領域種（seed region）を検索する方法及び領域種から領域を拡大すると供に平面を更新する方法について説明する。図２６は、領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。図２６に示すように、領域種の選択には、先ず、線分抽出の際に使用した行方向又は列方向のデータ列が隣接する３つの線分（ｌ１，ｌ２，ｌ３）であって、互いの線分（ｌ１，ｌ２），（ｌ２，ｌ３）における画素位置が上記データ列とは直交する方向にて重複したものを検索する（ステップＳ３１）。各データ点は画像内における画素位置を示すインデックス（index）を有しており、例えば行方向のデータ列における線分である場合、このインデックスを比較して列方向にて重複しているか否かを比較する。この検索に成功した場合（ステップＳ３２：ＹＥＳ）、上記式（７）を使用して上記（６−１）を算出する。これにより、平面パラメータｎ，ｄを決定でき、これを使用して上記式（８）に示す平面方程式の２乗平均誤差（ｌ１，ｌ２，ｌ３）を計算する（ステップＳ３３）。そして、この平面方程式の２乗平均誤差ｒｍｓ（ｌ１，ｌ２，ｌ３）が例えば１ｃｍなどの所定の閾値ｔｈｒｍｓ１より小さい場合には、この３つの線分を領域種として選択する（ステップＳ３４）。所定の閾値ｔｈｒｍｓ１より大きい場合には、再びステップＳ３１に戻り、上記条件を満たす線分を検索する。また、領域種に選ばれた線分は、線分群のリストから除くことで、他の平面拡張などの際に使用されないようにしておく。
こうして選択された領域種から線分拡張法により領域を拡張する。すなわち、先ず、領域種の領域に追加する候補となる線分を検索する（ステップＳ３５）。なお、この領域は、領域種が既に更新されている場合の、後述する更新された領域種も含む。候補となる線分は、領域種の領域に含まれる線分（例えばｌ１）に隣接する線分（ｌ４）であって、上述同様、これらの線分の画素位置が相互に重なりあうことを条件とする。検索が成功した場合（ステップＳ３６：ＹＥＳ）、その平面方程式の２乗平均誤差ｒｍｓ（ｌ４）を算出し、これが所定の閾値ｔｈｒｍｓ２より小さいか否かを判定し（ステップＳ３７）、小さい場合には平面パラメータを更新し（ステップＳ３８）、再びステップＳ３５からの処理を繰り返す。ここで、候補となる線分がなくなるまで処理を繰り返し、候補となる線分がなくなったら（ステップＳ３６：ＮＯ）、ステップＳ３１の処理に戻り、再び領域種を検索する。そして、線分群に含まれる領域種がなくなった場合（ステップＳ３２：ＮＯ）、今まで得られている平面パラメータを出力して処理を終了する。
ここで、本実施の形態においては、領域種を検索し、３つの線分が同一平面に属するか否かの判定、及び領域拡張処理を行う際に基準平面又はこれを更新した更新平面に属するか否かの判定には、上記式（８）を使用する。すなわち、平面方程式の２乗平均誤差ｒｍｓが所定の閾値（ｔｈｒｍｓ１）未満である場合にのみその線分（群）を同一平面に属するものと推定し、その線分を含めた平面として再び平面を算出する。このように平面方程式の２乗平均誤差ｒｍｓを使用して同一平面に属するか否かを判定することにより、更にノイズにロバストでかつ、細かい段差を含んでいるような場合にも正確に平面を抽出することができる。以下にその理由について説明する。
図２７は、その効果を示す図であって、端点と直線との距離が等しくても平面方程式の２乗平均誤差ｒｍｓが異なる例を示す模式図である。ここで、非特許文献４のように、領域拡張処理する際、注目の直線（線分）の端点（end point）と平面Ｐとの距離Ｄの値が所定の閾値より小さい場合に、当該注目の線分が平面Ｐと同一平面であるとして領域拡張処理を行うと、平面Ｐに交差する直線Ｌａ（図２７Ａ）と、平面Ｐと平行で所定距離ずれているような直線Ｌｂ（図２７Ｂ）とが同様に平面Ｐの更新に使用されることとなる。ここで、平面方程式の２乗平均誤差ｒｍｓを求めると、図２７Ａの直線Ｌａから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌａ）に比して図２７Ｂの直線Ｌｂから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌｂ）の方が大きい。すなわち、図２７Ａのように、直線Ｌａと平面Ｐとが交差する場合は、平面方程式の２乗平均誤差ｒｍｓが比較的小さくノイズの影響である場合が多いのに対し、図２７Ｂのような場合、平面方程式の２乗平均誤差ｒｍｓが大きく、直線Ｌｂは平面Ｐと同一平面ではなく異なる平面Ｐ’である確率が高い。したがって、複数の平面が含まれるような環境から平面を精確に求める必要がある場合などにおいては、本実施の形態のように、平面方程式の２乗平均誤差ｒｍｓを算出し、この値が所定の閾値（ｔｈｒｍｓ２）未満である場合に同一平面と判断することが好ましい。
なお、環境や距離データの性質に応じて、従来と同様、線分の端点と平面との距離が所定の閾値以下の場合は当該線分を平面に含めるようにしたり、これらを組み合わせてもよい。
上記の領域拡張における線分追加検定において利用される閾値（ｔｈｒｍｓ２）は、実験等によって獲得された定数を用いることも可能だが、閾値の不適切な設定によるUnder-Segmentation, Over-Segmentationの問題が生じる為、線分に含まれる距離データの性質によって適応的に決定されることが望ましい。ノイズを多く含む線分群に対して低い閾値を設定すると、多くの線分が異なる領域に分割され、領域拡張が適切に行われない。
そこで本実施の形態では、下記式（９）により閾値（ｔｈｒｍｓ２）を線分に含まれるノイズに応じて変化させることで、線分データの精度に応じた適応的な判定を可能とした。
ｔｈｒｍｓ２＝max（ｔｈｒｍｓ３，ｄｍａｈａsigma(l２)） …（９）
ここで、ｔｈｒｍｓ３は閾値（ｔｈｒｍｓ２）の下限を定義する定数、ｄｍａｈａはマハラノビス距離、sigma()は線分の分散を表す。ノイズを多く含むデータは、線分の分散sigma()が大きく、閾値（ｔｈｒｍｓ２）が大きくなり、領域拡張に関する許容範囲が大きくなる。
ここでは、sigma()として上記式（２）で表される直線方程式とデータ点の誤差の総和Ｅｆｉｔを利用し、閾値の下限ｔｈｒｍｓ３は領域種の検定で利用した線分の許容誤差閾値ｔｈｒｍｓ１と同じとした。
また、面パラメータｎ，ｄを一旦算出すれば、平面方程式の２乗平均誤差ｒｍｓは、データ点群について線分抽出の際に求めた２つのモーメントの値から平面方程式を更新し、上記式（８）にて簡単に算出することができる。
また、上述の領域種の選択方法は、図２８のようにも表現することができる。overlap(lj，lk)は、各イメージロウに含まれる直線ベクトルｌｊとｌｋにおける端点間の位置が直線ベクトルとは直交する位置にて重なっている場合にtrueを出力する関数である。また、fitPlane(l1，l2，l3)は、上記式（４）〜（７）によりＡｍ＝ｂの解を求め平面パラメータｎ，ｄを計算し、上記式（８）により算出されたＡ，ｂにより、直線ベクトルｌ１，ｌ２，ｌ３を平面にフィッティングさせる関数である。
rms(l1，l2，l3)は、上記式（６）を使用して３本の直線全てにおいて、平面方程式の２乗平均誤差ｒｍｓの値を算出する関数である。また、remove(l1，l2，l3)は、lines［i］，lines［i+1］l2，lines［i+2］から領域種を構成するとして選択されたそれぞれ直線l1，l2，l3を除くことを意味し、これにより、再びこれらの直線が計算に使用されることを防止する。
また、領域拡張処理は、図２９のように表現することもできる。図２７において、Ａ及びｂは、上記式（６−１）に示すそれぞれ行列及びベクトルである、また、add(A，b，l)は、上記式（８）により、Ａとｂに直線lineのモーメントを加える関数である。Solve(A，b)は、Ａｍ＝ｂを満たすｍを求め、上記式（４）〜（７）により平面パラメータｎ，ｄを計算する。select(open)は、例えば最初の１つなど、openの中から任意に１つのエレメントを選択する関数である。また、index(l1)は、画素列又は行におけるl1のインデックスを返す関数である。また、neighbor(index)は、与えられたインデックスに隣接したインデックス、例えば{index-1，index+1}を返す関数である。
また、上述したように、本実施の形態においては、図１９のステップＳ３において領域拡張処理を行って平面方程式を更新した後、ステップＳ４において平面方程式を再度算出する処理（Post processing）を行う。この再度算出する処理では、例えば上述のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離データ点又は線分の平面からのずれを計算し、所定の値以上平面から外れる距離データ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低減することができる。
次に、このステップＳ４について詳細に説明する。ここでは、２つのステップにより、平面方程式を再度算出する方法について説明する。先ず、ステップＳ３にて検出された各平面の境界の距離データ点（pixels）において、現在属している平面よりも、隣接する平面までの距離が近いデータ点が検出された場合は、当該データ点を隣接する平面の方に含める処理をする。また、いずれの平面にも属していなく、且つ距離が例えば１．５ｃｍなど比較的大きい閾値以下である平面が存在するデータ点が検出できた場合は、当該データ点をその平面に含める処理をする。これらの処理は各平面領域の境界近傍のデータ点を検索することで実行することができる。以上の処理が終了したら、再度平面方程式を算出する。
次に、上述のようにして再度算出された平面の各領域の境界近傍において、各データ点と平面との距離が例えば０．７５ｃｍなど比較的小さい閾値を超える場合は、それらのデータ点を捨てる処理を実行する。これにより、その平面領域は若干小さくなるものの更に精確な平面を求めることができる。距離データ点を削除後、再び平面を求め、この処理を繰り返す。このことにより、極めて精密に平面を求めることができる。
次に各処理によって得られる結果を示す。図３０Ａは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図３０Ｂは、縦軸をｘ、横軸をｙ、各データ点の濃淡でｚ軸を表現して３次元距離データを示す図であり、更に、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す。図３０Ｂに示す直線群から領域拡張処理によりえられた平面領域を図３０Ｃに示す。このように、ロボット装置の視野内には、１つの平面（床面）のみが存在する、すなわち、床面が全て同じ平面として検出されていることがわかる。
次に、床面に段差を一段置いたときの結果を図３１に示す。図３１Ａに示すように、床面Ｆには、１段の段差ＳＴ３が載置されている。図３１Ｂは、実験条件を示す図であり、着目点と直線（線分）との距離がmax_dを超える場合は、データ点群を分割する。また、抽出の成否（水平）（correct extraction(horizontal)）は、行方向のデータ列毎に、合計１０回の線分抽出を行う線分拡法による平面検出を行って成功した回数を示し、抽出の成否（垂直）（correct extraction(vertical)）は、列方向のデータ列毎についての抽出の成否を示す。また、Ｎｏ．１〜Ｎｏ．５は、上述したZig-Zag-Shape判別処理を取り入れていない従来の線分拡張法による平面検出処理の条件、Ｎｏ．６は、Zig-Zag-Shape判別処理を行った本実施の形態における平面検出方法の条件を示す。
図３１Ｃ及び図３１Ｄは、線分拡張法により平面検出した結果を示す図であって、それぞれ本実施の形態における手法により平面検出した結果、従来の線分拡張法により平面検出した結果（比較例）を示す。図３１Ｂに示すように、従来の手法においては、線分抽出（Line Fitting）において推定のための閾値パラメータmax_dを大きくする（max_d＝２５，３０）と検出率が下がり、閾値max_d小さくする(max_d＝１０，１５)と検出率が向上する。これに対して、本発明のように、ジグザグ形検証処理を導入することにより、大きな閾値max_d＝３０を設定しても、優れた検出結果を示すことがわかる。
すなわち、閾値max_dを大きくすると、ノイズの影響が少なくなるものの、線分抽出が難しくなり、閾値max_dを小さくすると、ノイズの影響を受けて誤検出が多くなってしまう。図３２Ａに示す床面を撮影した画像から３次元距離データを取得した場合を図３２Ｂ及び図３２Ｃに示す。いずれ左図は、行方向の画素列（距離データ列）から線分を抽出した例、右図は列方向の画素列（距離データ列）から線分を抽出した例を示す。図３２Ｂに示すように、閾値max_dを小さくすると、ノイズの影響が大きくなり、ノイズの影響が大きい遠方などにおいては特に、線分をうまく検出することができない。一方、図３２Ｃに示すように、従来の線分抽出に更にジグザグ形判別処理を加えた場合、閾値max_dを大きくしても、更にノイズの影響が大きい遠方の領域であっても線分が検出されていることがわかる。
図３３及び図３４は、それぞれ異なる階段を撮影した画像から３次元距離データを取得して平面検出した例を示す図である。図３３及び図３４に示すように、いずれの場合も全ての踏面を平面として検出できている。また、図３４Ｂは、床面の一部も他の平面として検出成功していることを示す。
本実施の形態によれば、線分拡張法による平面検出を行う際、始めは大きな閾値を設定して線分を分割し、次にZig-Zag-Shape判別処理により、閾値を超えるデータ点を持たない直線であってもジグザグ形である場合には、ノイズではなく、複数平面からなる直線であるとして線分を分割するようにしたので、ノイズを含む距離情報から複数の平面を精度よく検出することが可能となる。
このように、小さい段差も精度よく検出することができるため、例えばロボット装置が移動可能な環境内の階段などを認識することができ、二足歩行ロボット装置であれば、この検出結果を利用して階段昇降動作が可能となる。
更に、複数の平面によって構成されている凸凹の床面を歩行可能な平面だと誤認識することがなくなり、ロボット装置の移動などが更に簡単になる。
なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。また、上述した線分抽出処理、ジグザグ形検証処理、領域拡大処理などの各処理のうち１以上の任意の処理は、ハードウェアで構成しても、演算器（ＣＰＵ）にコンピュータプログラムを実行させることで実現してもよい。コンピュータプログラムとする場合には、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。
Claims (21)
- １．３次元の距離データから平面を検出する平面検出装置において、
３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、
上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、
上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする平面検出装置。 - ２．上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - ３．上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - ４．上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - ５．上記線分抽出手段は、上記第１の線分を求めた距離データ点群の標準偏差が所定の閾値以上である場合には、当該距離データ点群を分割する
ことを特徴とする請求の範囲第４項記載の平面検出装置。 - ６．上記平面領域拡張手段は、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - ７．上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有する
ことを特徴とする請求の範囲第６項記載の平面検出装置。 - ８．上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定する
ことを特徴とする請求の範囲第６項記載の平面検出装置。 - ９．上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から最小二乗法により上記第２の線分を生成する
ことを特徴とする請求の範囲第４項記載の平面検出装置。 - １０．上記線分抽出手段は、２つの撮像手段による視差によって距離を計測する距離計測手段により計測された３次元の距離データを使用して線分抽出する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - １１．上記線分抽出手段は、レーザ距離計測計により計測された３次元の距離データを使用して線分抽出する
ことを特徴とする請求の範囲第１項記載の平面検出装置。 - １２．３次元の距離データから平面を検出する平面検出方法において、
３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、
上記線分抽出工程にて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、
上記線分抽出工程では、距離データ点の分布に応じて適応的に線分を抽出する
ことを特徴とする平面検出方法。 - １３．上記線分抽出工程では、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定する
ことを特徴とする請求の範囲第１２項記載の平面検出方法。 - １４．上記線分抽出工程では、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割する
ことを特徴とする請求の範囲第１３項記載の平面検出方法。 - １５．上記線分抽出工程では、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割する
ことを特徴とする請求の範囲第１３項記載の平面検出方法。 - １６．上記平面領域拡張工程では、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する
ことを特徴とする請求の範囲第１２項記載の平面検出方法。 - １７．上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出工程を更に有する
ことを特徴とする請求の範囲第１６項記載の平面検出方法。 - １８．上記平面領域拡張工程では、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定する
ことを特徴とする請求の範囲第１６項記載の平面検出方法。 - １９．自律的に行動するロボット装置において、
３次元の距離データを取得する距離計測手段と、
３次元の距離データから平面を検出する平面検出装置と、
上記平面検出装置による平面検出結果に基づき行動を制御する行動制御手段とを有し、
上記平面検出装置は、
３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、
上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、
上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出する
ことを特徴とするロボット装置。 - ２０．対象物に対して模様を付与する模様付与手段を有する
ことを特徴とする請求の範囲第１９項記載のロボット装置。 - ２１．上記模様付与手段は、上記３次元の距離データを取得する際に上記対象物に模様を投射する投射手段である
ことを特徴とする請求の範囲第２０項記載のロボット装置。
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2004077215 | 2004-03-17 | ||
| JP2004077215 | 2004-03-17 | ||
| PCT/JP2005/004839 WO2005088244A1 (ja) | 2004-03-17 | 2005-03-17 | 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPWO2005088244A1 true JPWO2005088244A1 (ja) | 2008-01-31 |
| JP4636016B2 JP4636016B2 (ja) | 2011-02-23 |
Family
ID=34975694
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2006511066A Expired - Fee Related JP4636016B2 (ja) | 2004-03-17 | 2005-03-17 | 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 |
Country Status (3)
| Country | Link |
|---|---|
| US (1) | US8289321B2 (ja) |
| JP (1) | JP4636016B2 (ja) |
| WO (1) | WO2005088244A1 (ja) |
Families Citing this family (35)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102006008275A1 (de) * | 2006-02-22 | 2007-08-23 | Siemens Ag | Verfahren zur Detektion von Objekten mit einer schwenkbaren Sensoreinrichtung |
| US7916935B2 (en) * | 2006-09-19 | 2011-03-29 | Wisconsin Alumni Research Foundation | Systems and methods for automatically determining 3-dimensional object information and for controlling a process based on automatically-determined 3-dimensional object information |
| FR2929873B1 (fr) * | 2008-04-09 | 2010-09-03 | Aldebaran Robotics | Architecture de controle-commande d'un robot mobile utilisant des membres articules |
| KR101495333B1 (ko) * | 2008-07-02 | 2015-02-25 | 삼성전자 주식회사 | 장애물 검출 장치 및 방법 |
| JP5499261B2 (ja) * | 2009-07-16 | 2014-05-21 | 日本信号株式会社 | 画像処理装置 |
| JP5417645B2 (ja) * | 2010-03-08 | 2014-02-19 | オプテックス株式会社 | 距離画像における平面推定方法および距離画像カメラ |
| JP5161936B2 (ja) * | 2010-08-11 | 2013-03-13 | 株式会社パスコ | データ解析装置、データ解析方法、及びプログラム |
| JP5588332B2 (ja) * | 2010-12-10 | 2014-09-10 | 東芝アルパイン・オートモティブテクノロジー株式会社 | 車両用画像処理装置および車両用画像処理方法 |
| JP2013047662A (ja) * | 2011-07-27 | 2013-03-07 | Ihi Corp | 対象物体の検出方法、検出装置及びプログラム |
| KR101820299B1 (ko) * | 2011-11-23 | 2018-03-02 | 삼성전자주식회사 | 3차원 데이터 영상의 계단 인식 방법 |
| US9269155B2 (en) * | 2012-04-05 | 2016-02-23 | Mediatek Singapore Pte. Ltd. | Region growing method for depth map/color image |
| US9582932B2 (en) * | 2012-06-05 | 2017-02-28 | Apple Inc. | Identifying and parameterizing roof types in map data |
| JP6121216B2 (ja) * | 2013-03-29 | 2017-04-26 | 株式会社パスコ | 多平面構造物の凹凸抽出装置、多平面構造物の凹凸抽出方法、及びプログラム |
| JP6032678B2 (ja) * | 2013-06-20 | 2016-11-30 | 株式会社パスコ | データ解析装置、データ解析方法、及びプログラム |
| US9996974B2 (en) * | 2013-08-30 | 2018-06-12 | Qualcomm Incorporated | Method and apparatus for representing a physical scene |
| JP6259262B2 (ja) * | 2013-11-08 | 2018-01-10 | キヤノン株式会社 | 画像処理装置および画像処理方法 |
| US9412040B2 (en) * | 2013-12-04 | 2016-08-09 | Mitsubishi Electric Research Laboratories, Inc. | Method for extracting planes from 3D point cloud sensor data |
| US20200409382A1 (en) * | 2014-11-10 | 2020-12-31 | Carnegie Mellon University | Intelligent cleaning robot |
| WO2016084389A1 (ja) * | 2014-11-28 | 2016-06-02 | パナソニックＩｐマネジメント株式会社 | モデリング装置、３次元モデル生成装置、モデリング方法、プログラム |
| JP6597352B2 (ja) * | 2016-02-04 | 2019-10-30 | 株式会社デンソー | 物体認識装置 |
| JP2017181291A (ja) * | 2016-03-30 | 2017-10-05 | 富士通株式会社 | 距離測定装置、距離測定方法及びプログラム |
| WO2018108832A1 (en) * | 2016-12-14 | 2018-06-21 | Starship Technologies Oü | Robot, system and method detecting and/or responding to transitions in height |
| US10077047B2 (en) | 2017-02-10 | 2018-09-18 | Waymo Llc | Using wheel orientation to determine future heading |
| CN108510540B (zh) * | 2017-02-23 | 2020-02-07 | 杭州海康威视数字技术股份有限公司 | 立体视觉摄像机及其高度获取方法 |
| JP7148229B2 (ja) * | 2017-07-31 | 2022-10-05 | 株式会社トプコン | 三次元点群データの縦断面図作成方法，そのための測量データ処理装置，および測量システム |
| US10989795B2 (en) * | 2017-11-21 | 2021-04-27 | Faro Technologies, Inc. | System for surface analysis and method thereof |
| US10902625B1 (en) * | 2018-01-23 | 2021-01-26 | Apple Inc. | Planar surface detection |
| WO2019241262A1 (en) * | 2018-06-11 | 2019-12-19 | Jabil Inc. | Apparatus, system, and method of docking for autonomous robot navigation |
| US11599128B2 (en) * | 2020-04-22 | 2023-03-07 | Boston Dynamics, Inc. | Perception and fitting for a stair tracker |
| US11548151B2 (en) | 2019-04-12 | 2023-01-10 | Boston Dynamics, Inc. | Robotically negotiating stairs |
| CN110216661B (zh) * | 2019-04-29 | 2020-12-22 | 北京云迹科技有限公司 | 跌落区域识别的方法及装置 |
| JP7395856B2 (ja) * | 2019-07-01 | 2023-12-12 | セイコーエプソン株式会社 | 印刷制御装置、印刷制御プログラム、及び、印刷物生産方法 |
| US11796637B1 (en) * | 2020-09-10 | 2023-10-24 | Amazon Technologies, Inc. | Fall detection on uneven surfaces using radar |
| CN113175987A (zh) * | 2021-04-09 | 2021-07-27 | 东南大学 | 一种考虑环境温度变异的桥梁动力特性异常预警方法 |
| CN113390431B (zh) * | 2021-06-17 | 2022-09-30 | 广东工业大学 | 动态生成参考线的方法、装置、计算机设备和存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0793541A (ja) * | 1993-05-26 | 1995-04-07 | Matsushita Electric Works Ltd | 形状認識方法 |
| JP2001062566A (ja) * | 1999-08-30 | 2001-03-13 | Kobe Steel Ltd | 溶接線位置検出装置 |
Family Cites Families (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH03176701A (ja) | 1989-12-05 | 1991-07-31 | Toshiba Corp | Ｎ対１バックアップコントローラ |
| JPH03278467A (ja) | 1990-03-27 | 1991-12-10 | Canon Inc | 薄膜半導体装置 |
| JP3192736B2 (ja) | 1992-02-10 | 2001-07-30 | 本田技研工業株式会社 | 移動体の階段などの認識方法 |
| JP3176701B2 (ja) * | 1992-04-15 | 2001-06-18 | 本田技研工業株式会社 | 移動体の現在位置認識処理装置 |
| JP3278467B2 (ja) | 1992-08-18 | 2002-04-30 | 本田技研工業株式会社 | 移動ロボットの制御装置 |
| JP3330710B2 (ja) * | 1993-12-30 | 2002-09-30 | 本田技研工業株式会社 | 移動ロボットの位置検知および制御装置 |
| JPH08161493A (ja) * | 1994-12-08 | 1996-06-21 | Mazda Motor Corp | 線形状検出方法およびその装置 |
| US5978504A (en) * | 1997-02-19 | 1999-11-02 | Carnegie Mellon University | Fast planar segmentation of range data for mobile robots |
| JP3945279B2 (ja) * | 2002-03-15 | 2007-07-18 | ソニー株式会社 | 障害物認識装置、障害物認識方法、及び障害物認識プログラム並びに移動型ロボット装置 |
| US20040138780A1 (en) * | 2002-11-15 | 2004-07-15 | Lewis Murray Anthony | Certain principles of biomorphic robots |
| JP3994950B2 (ja) * | 2003-09-19 | 2007-10-24 | ソニー株式会社 | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 |
| US7653216B2 (en) * | 2003-12-23 | 2010-01-26 | Carnegie Mellon University | Polyhedron recognition system |
| WO2005087452A1 (ja) | 2004-03-17 | 2005-09-22 | Sony Corporation | ロボット装置、及びその動作制御方法、並びに移動装置 |
-
2005
- 2005-03-17 WO PCT/JP2005/004839 patent/WO2005088244A1/ja active Application Filing
- 2005-03-17 JP JP2006511066A patent/JP4636016B2/ja not_active Expired - Fee Related
- 2005-03-17 US US10/593,150 patent/US8289321B2/en not_active Expired - Fee Related
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0793541A (ja) * | 1993-05-26 | 1995-04-07 | Matsushita Electric Works Ltd | 形状認識方法 |
| JP2001062566A (ja) * | 1999-08-30 | 2001-03-13 | Kobe Steel Ltd | 溶接線位置検出装置 |
Non-Patent Citations (1)
| Title |
|---|
| JPN6010025224, Ｘｉａｏｙｉ Ｊｉａｎｇ， ｅｔ ａｌ．, ""Ｆａｓｔ ｓｅｇｍｅｎｔａｔｉｏｎ ｏｆ ｒａｎｇｅ ｉｍａｇｅｓ ｉｎｔｏ ｐｌａｎａｒ ｒｅｇｉｏｎｓ ｂｙ ｓｃａｎ ｌｉｎｅ ｇｒｏｕｐｉｎｇ"", Ｍａｃｈｉｎｅ Ｖｉｓｉｏｎ ａｎｄ Ａｐｐｌｉｃａｔｉｏｎｓ, 199406, Ｖｏｌ．７，Ｎｏ．２, ｐ．１１５−１２２ * |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2005088244A1 (ja) | 2005-09-22 |
| US20070257910A1 (en) | 2007-11-08 |
| WO2005088244A9 (ja) | 2008-03-13 |
| US8289321B2 (en) | 2012-10-16 |
| JP4636016B2 (ja) | 2011-02-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4636016B2 (ja) | 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 | |
| JP4479372B2 (ja) | 環境地図作成方法、環境地図作成装置、及び移動型ロボット装置 | |
| JP4618247B2 (ja) | ロボット装置及びその動作制御方法 | |
| JP3994950B2 (ja) | 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 | |
| KR101776622B1 (ko) | 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
| US7769491B2 (en) | Obstacle avoiding apparatus, obstacle avoiding method, obstacle avoiding program, and mobile robot apparatus | |
| Saeedi et al. | Vision-based 3-D trajectory tracking for unknown environments | |
| Sabe et al. | Obstacle avoidance and path planning for humanoid robots using stereo vision | |
| KR101708659B1 (ko) | 이동 로봇의 맵을 업데이트하기 위한 장치 및 그 방법 | |
| Pérez-Yus et al. | Detection and modelling of staircases using a wearable depth sensor | |
| KR101650799B1 (ko) | 가변 자세를 포함하는 이미지를 컴퓨터를 사용하여 실시간으로 분석하는 방법 | |
| Moosmann et al. | Motion estimation from range images in dynamic outdoor scenes | |
| KR20110139694A (ko) | 제스쳐 인식 방법 및 시스템 | |
| JP6708260B2 (ja) | 情報処理装置、情報処理方法、およびプログラム | |
| JP2007041656A (ja) | 移動体制御方法および移動体 | |
| JP2003271975A (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置 | |
| JP4535096B2 (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 | |
| JP6410231B2 (ja) | 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム | |
| JP7145770B2 (ja) | 車間距離測定装置、誤差モデル生成装置および学習モデル生成装置とこれらの方法およびプログラム | |
| JP2021070122A (ja) | 学習データ生成方法 | |
| JP2007041657A (ja) | 移動体制御方法および移動体 | |
| Ma et al. | A probabilistic framework for stereo-vision based 3d object search with 6d pose estimation | |
| JP4407244B2 (ja) | ロボット装置及びその物体学習方法 | |
| JP2003346150A (ja) | 床面認識装置及び床面認識方法並びにロボット装置 | |
| Belter et al. | On-Board Perception and Motion Planning for Legged Locomotion over Rough Terrain. |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20100511 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20100705 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20100810 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20101001 |
|
| 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: 20101026 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20101108 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131203 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131203 Year of fee payment: 3 |
|
| LAPS | Cancellation because of no payment of annual fees |