WO2005088244A1

WO2005088244A1 - 平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置

Info

Publication number: WO2005088244A1
Application number: PCT/JP2005/004839
Authority: WO
Inventors: Steffen Gutmann; Masaki Fukuchi; Kohtaro Sabe; Kenta Kawamoto
Original assignee: Sony Corporation
Priority date: 2004-03-17
Filing date: 2005-03-17
Publication date: 2005-09-22
Also published as: US8289321B2; JP4636016B2; WO2005088244A9; US20070257910A1; JPWO2005088244A1

Abstract

　平面検出装置の平面検出部（３）は、画像を構成する距離データ点から同一平面にある距離データ点群を選択し、この距離データ点群から線分を抽出する線分抽出部（４）と、画像内に含まれる、線分抽出部（４）によって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部（５）とを有する。線分抽出部（４）は、まず、距離データ点群の端点を結ぶ線分Ｌ１を引き、この線分Ｌ１との距離が最も大きい着目点brkを探し、所定の値以上の場合は着目点でデータ点群を分割し、距離が所定の値未満の場合は最小二乗法により線分Ｌ２を求める。そして線分Ｌ２の一方の側にデータ点が所定の数以上連続して存在する場合、ジグザグ形と判断し、着目点brkでデータ点群を分割して、再び以上の処理を繰り返す。これによって、計測ノイズを含む距離データからノイズに対してロバストにかつ複数平面を同時に正確に検出する。

Description

平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置

技術分野

[0001] 本発明は、 3次元距離データから平面を検出する平面検出装置、平面検出方法及び平面検出装置を搭載したロボット装置に関し、特に、線分拡張法 (scan line grouping)により平面を検出する平面検出装置、平面検出方法及びロボット装置に関する。

本出願は、日本国において 2004年 3月 17日に出願された日本特許出願番号 200 4-077215を基礎として優先権を主張するものであり、これらの出願は参照することにより、本出願に援用される。

背景技術

[0002] 3次元の距離情報から平面を検出するアルゴリズムについては様々な研究が行われている。検出された平面は、例えば移動式のロボット装置の障害物回避、または階段昇降動作の用途とすることができる。

一般に、距離情報カゝら平面を検出する手法は、以下の手順により構成されている。 1. 3次元距離情報を取得する

2.複数の平面に分割する

3.各平面に関して、重心、平面方程式、境界等を計算する

4. 2、 3の処理を繰り返し行い、最適な分割方法を獲得する

例えば、図 1Aに示す床面に載置された階段を有する画像は、図 1Bに示すように、 4つの平面領域 A、 B、 C、 Dに分割される。ここで、領域 Aは床部分、領域 B、 C、 Dは階段部分を示す。

次に、 3次元の距離情報から平面を検出する従来の手法を下記文献の技術を例にとって説明する。例えば、 Hooverら（A. Hoover, G. Jean- Baptiste, X. Jiang, P.J. Flynn, H. Bunke, D. Goldgof, K. Bowyer, D. Eggert, A. Fitzgiobon, and R. Fisher.) , 「距離画像セグメンテーションアルゴリズムの実験的比較 (An experimental comparison of range image segmentation algorithms)」 , Transaction on Pattern Analysis Machine Intelligence, 18(7), 1996では、距離情報から参照点をランダムにサンプルし、その点力も空間的に近傍の点を連結することによって平面を検出する手法を案し飞いる。ま 7こ、 Iocchiら (L. Iocchi, K. Konolige, and M. Bajracnarya.) , 「ステレオ画像を使用した環境平面における現実の視覚地図（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.し onf. on Robotics and Automation (ICRA), 2001では、距離情報からランダムに選んだ 3点を平面方程式に当てはめ、統計的に平面を検出するランダマイズドハフ変換による手法を提案している。更にまた、 Jiangら（X.-Y. Jiang and H. Bunke.)「線分拡張法によって距離画像を平面領域に尚速に分害 (Iする牛法 (Fast segmentation of range images into planar regions by scan line grouping)」 Machine Vision and Applications, 7(2), PI 15 122, 1994では、距離画像中の列に並んだ点群はそれらの点が 3次元空間で同一平面上にあるならば、空間内で直線となることを利用し、線分拡張法 (scan line grouping)による手法を提案している。

ところで、上述の各文献などのように、距離情報を使用した平面検出において重大な問題となるのが、ノイズに起因する、 under- segmentation, over- segmentationの問題である。

under- segmentationとは、実際は複数の平面が存在するのにも拘わらず、ノイズの影響などによりそれらを例えば平均化したような 1つの平面として認識してしまうことをいい、 over- segmentationとは、実際は 1つの同一平面であるのにも拘わらず、ノイズの影響などにより複数の異なる平面として認識してしまうものである。

図 2に示すように、カメラ 401RZLにより取得された距離画像には、複数の踏面や、側面、床面などが含まれる。例えば図 2の上図に示すように、例えばカメラ 401RZ Lの視野内に階段 400が存在する場合、距離画像内には複数の平面を含む。したがつて、図 2下図に示すように、 X— z平面であれば、踏面 402、側面 403などの複数の平面を含むが、 under- segmentationにより、これら複数の平面を区別できず、一つの平面 403として検出してしまう。平面検出器は、一般に、平面の検出要求精度に比べてノイズの影響が大きい計測データ力平面を検出するため、これを設計する際には、複数平面に分離する為の閾値を甘くしなければならず、従ってこのような under- segmentationの問題が起こりやすい。また、逆に、ノイズの影響が大きい計測データの場合に閾値を下げると、実際には一つの平面が複数の平面に分離されてしまつ over— segmentation力 S起こる。

上述の Hooverらの文献や、 Iocchiらの文献などのように、ハフ変換によって平面を推定する手法の場合、 under- segmentationの問題が非常に生じやすい。図 3A—図 3Dは、ハフ変換により平面を抽出する方法を説明する図であり、図 3Aは階段を示す図、図 3Bは図 3Aに示す階段力も得られた 3次元距離データ、図 3Cは図 3Bの距離データをノヽフ変換してピークを求めた図、図 3Dは図 3Cに示すピークが示す平面と、実際の平面との比較を示す図である。図 3A— Dに示すように、視野内に階段 410を含む場合、その 3次元データは、図 3Bに示すものとなる。このデータの 3点をランダムに選択して平面を求め、これを平面パラメータ空間に投票することでヒストグラムを生成し、図 3Cに示すように支配的な平面がピーク Pとして検出でき、このピークが示すパラメータによって決定される平面が視野内に存在する平面として推定される。し力しながら、ハフ変換した後のデータ力も統計的に平面を推定すると、 under- segmentationした結果が統計的に最も支配的な値となる。すなわち、図 3Dに示すように、検出された平面 411は、実際には全ての平面 412、 413、 414を均した平面として求まってしまう。このように、ハフ変換は、視野内に含まれる支配的な平面を推定、検出することはできても、複数平面が存在する場合にそれらを精度よく検出することができない。

また、 Jiangらの文献には、線分拡張法（scan line grouping)による平面抽出手法が開示されている。線分拡張法による平面検出では、まず、撮影された画像から 3次元距離データを取得し、この 3次元距離データにおいて、行方向または列方向のデータ列（image row:イメージロウ）毎に以下の処理を行う。例えば、画像内の行方向のデータ列において、 3次元空間において同一の平面に属するならば同一の直線となるとことを利用し、同一平面に属するデータ点群力も線分を生成する。そして、生成された線分群において、同一平面を構成する隣接する 3本の線分を抽出して基準となる平面を求め、この基準となる平面に隣接する線分が同一平面に属する場合に当該隣接する線分によって基準となる平面の領域を拡大するとともに基準となる平面を更新することにより、平面を検出する方法である。

図 4は、線分拡張法による平面検出処理を示すフローチャートである。図 4に示すように、先ず、距離画像を入力し (ステップ S41)、距離画像を構成する行方向又は列方向の各データ列において同一の平面上にあると推定されるデータ点群力線分を生成する (ステップ S42)。そして、生成された線分群の中から平面の種となる領域（以下、領域種 (seed region)という。）を検索し、該当する領域種を選択する (ステップ S43, S44)。この選択においては、上下隣接する 1ラインが同一平面にあることを条件とする。そして、選択した 3本の線分力なる領域種が属する平面を 3本の線分から平均して求めておく。

そして、ステップ S3にて選択した領域種に隣接するデータ列において、その領域種と同一平面にある線分がある力否かを検索する。同一平面にある力否かは、空間的な距離を比較することで判断する。同一平面にあると判断された線分が存在する場合には、当該線分をこの領域種の領域に追加し (領域拡張処理)、追加した線分を含めたものとして、元の平面を更新し (平面更新処理）、これらの処理を繰り返し行ことで、領域を拡張するとともに平面を更新する (ステップ S45)。更に、ステップ S43 一 S45の処理を、種となる領域が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結し (ステップ S46)、処理を終了する。

この図 4に示す各処理について更に詳細に説明する。先ず、ステップ S2に示す線分抽出方法について説明する。図 5は、線分を抽出する処理を説明する図であって、図 5A—図 5Cは、その処理工程順に示す図である。図 5Aに示すように、先ず、与えられた複数のデータ点群 430の両端 430a, 430bを結ぶ線分 (弦)を生成する。そして、得られた線分 431との距離が最も大きいデータ点を検索する。検索されたデータ点 430cと線分 431との距離 dがある閾値を超える場合には、線分 431を分割する処理を行う。具体的には、図 5Bに示すように、線分 431を、左端のデータ点 430aと分割点となるデータ点 430cとを結ぶ線分 43 laと、分割点 430cと右端のデータ点 4 30bとを結ぶ線分 431bとに分割する。これを全ての点と線分との距離が閾値以下になるまで繰り返すことによって、与えられたデータにフィットした複数の線分を検出することができる。ここでは、図 5Cに示すように、最終的に 2箇所の分割点となるデータ点 430c, 430d力選択され、線分 431は、線分 431a、 431c, 431dの 3本の線分に分割される。

図 25は、ステップ S45に示す領域拡張処理を説明するための図である。上述の線分抽出処理によって得られた線分を、種となる領域力順次統合していくことによつて領域を分割することができる。例えば、図 25に示すように、画像 30内に複数の平面力なる階段 31が存在する場合、例えば太線で示す 32a— 32cの 3本の線分が領域種として選択されたとする。これら 3本の線分 32a— 32cからなる領域が領域種となる。先ず、この 3つの線分 32a— 32cにより 1つの平面（基準平面） Pを求める。次に、領域種の最も外側の線分 32a又は 32cに領域種外にて隣接するそれぞれデータ列 33又は 34において、平面 Pと同一の平面である線分を選択する。ここでは、線分 33 aが選択されるとする。次に、これら 4本の線分群からなる平面 P'を求め、基準平面 P を更新する。次に、線分 34aが選択されれば、 5本の線分群力もなる平面 P"を求め、平面 P'を更新する。これを繰り返すことにより、階段 31の 2段目の踏面が、破線で囲まれる平面 45として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像 30内から領域種となる 3つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる 3つの線分がなくなるまで図 4のステップ S43— S45の処理を繰り返す。

し力しながら、この線分拡張法による平面検出アルゴリズムにおいても、線分抽出の際の閾値の決定が非常に困難であるため、計測データのノイズの影響を排除することが極めて難しい。図 6は、 2つの閾値設定をした場合における線分抽出処理の結果の相違を示す図である。図 6Aは、ノイズが少ない計測データ点群 450の場合、図 6B は、ノイズが多い計測データ点群 460の場合であり、それぞれの場合において、上述の線分分割の際の閾値として大きい値 (Large threshold)を適用した場合と、小さい値 (Small threshold)を適用した場合の結果を示す。

図 6Aに示すように、ノイズが少なヽ計測データ点群 450から線分を抽出する場合、大きな閾値を設定すると under- segmentationが生じてしまうため、小さい閾値のときの方がよい抽出結果を示す。一方、図 6Bに示すように、ノイズが多いデータ点群 460 から線分を抽出する場合は、小さな閾値を設定すると、 over-segmentationの問題が発生してしまうため、大きな閾値を設定する必要がある。すなわち、小さい閾値ではノィズの影響を受けて線分が過剰に細力べ分割されてしまい、本来抽出すべき線分を抽出することができない。

一般に、ステレオビジョンなどの距離計測装置の場合、近くの計測データに関しては計測精度が高くノイズが少な、計測データを取得することができ、遠くの計測データに関しては、計測精度が低いためノイズが多い計測データとなってしまう。このため、距離に応じて適応的に閾値を決めることが望まれるが、環境による計測精度の違いの影響もあり、これを一意に決めるのは極めて困難である。

以上、まとめると、ランダマイズドハフ変換などによる平面検出は、支配的な平面を検出するには適している力階段など複数平面を含むデータ力複数の平面を検出するには、 under-segmentationの問題が生じてしまうという問題点があり、また線分拡張法を使用して平面を抽出する場合は、線分抽出の際の閾値の設定が難しぐ over- segmentation及び under- segmentationの問題が生じてしまヽ、 V、ずれの方法においても、距離計測データ力正確に平面を検出することが困難であるという問題点がある。

発明の開示

発明が解決しょうとする課題

本発明は、このような従来の実情に鑑みて提案されたものであり、計測ノイズを含む距離データ力ノイズに対してロバストにかつ複数平面を同時に正確に検出すること力できる平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置を提供することを目的とする。上述した目的を達成するために、本発明に係る平面検出装置は、 3次元の距離デ一タカ平面を検出する平面検出装置において、 3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。本発明においては、線分抽出手段は、 3次元の距離データが同一平面上にある場合同一直線上に並ぶことを利用して線分を抽出するが、この際、ノイズなどの影響により距離データ点の分布に違いが生じるため、この距離データの分布に応じて適応的に線分を抽出する（Adaptive Line Fitting)ことにより、ノイズに対してロバストに、精確な線分抽出を可能とし、抽出された多数の線分力線分拡張法により平面を求めるため、ノイズの影響などにより、本来複数平面が存在するのに 1つの平面としたり、 1 つの平面しか存在しないのに複数平面としたりすることなく精確に平面抽出することができる。

また、上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にある力否かを再度推定することができ、距離データ点の 3次元空間における距離に基づき一旦距離データ点群を抽出しておき、データ点の分布に基づき再度同一平面上にある力否かを推定することにより精確に線分抽出することができる。

更に、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割することができ、距離データ点の分布に偏りがある場合には、抽出した距離データ点群は同一の平面上にないと判断して着目点にて分害 ijすることができる。

更にまた、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第 1の線分を抽出し、該距離データ点群のうち該第 1の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第 2の線分を抽出し、該第 2の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することができ、例えば抽出したデータ点群の端点を結ぶ線分を第 1の線分とし、上記距離が大きい点が存在する場合には、例えば最小二乗法により第 2の線分を生成し、この第 2の線分において一方側に連続して複数のデータ点が存在する場合には、データ点群は例えば線分に対してジグザグな形などをとつていることが想定でき、従って抽出したデータ点群には偏りがあると判断して、上記着目点などにてデータ点群を分割することができる。

また、上記平面領域拡張手段は、同一の平面に属すると推定される 1以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力することができ、同一平面に属するとされる線分により平面領域拡張処理及び平面更新処理を行うことができる。

更に、上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除!ヽた距離データ点群力も再度平面を算出する平面再算出手段を更に有することができ、更新済平面はそれに属する全線分の平均した平面として得られているため、これ力大きく外れた距離データ点を除いたデータ点群力再度平面を求めることで、よりノイズなどの影響を低減した検出結果を得ることができる。

更に、上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定することができ、例えば平面方程式の 2乗平均誤差などに基づきノイズの影響であるの力、異なる平面なのかを判別して更に正確に平面検出することができる。

本発明に係る平面検出方法は、 3次元の距離データから平面を検出する平面検出方法において、 3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、上記線分抽出工程にて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、上記線分抽出工程では、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。

本発明に係るロボット装置は、自律的に行動するロボット装置において、 3次元の距離データを取得する距離計測手段と、 3次元の距離データから平面を検出する平面検出装置と、上記平面検出装置による平面検出結果に基づき行動を制御する行動制御手段とを有し、上記平面検出装置は、 3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によつて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分力平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする。

また、対象物に対して模様を照射する照射手段などの模様付与手段を有することができ、距離計測手段力ステレオカメラなどにより視差を利用して距離画像を取得するものである場合、観察対象となる階段、床面などの対象物が模様 (テクスチャ)がない又は不十分であると、うまく距離画像を得ることができないため、この模様付与手段により模様を付与して精確な距離画像を取得することができる。

本発明に係る平面検出装置及び方法によれば、線分拡張法により平面検出する際、距離データ点の分布に応じて適応的に線分を抽出することで、ノイズを含む距離データであっても精確に線分抽出することができ、この線分を使用して線分拡張法により平面検出することで、極めて精確に平面を検出することができる。

また、本発明に係るロボット装置は、上述の平面検出装置を搭載することにより、口ボット装置が具備する距離計測手段にてノイズを含む距離データが取得されても精確に平面検出することができ、ロボット装置の周囲環境内に存在する階段を検出して昇降動作をさせたり、床面の段差などを認識して段差がある床面上を移動することなどが可能となり、更にエンターテイメント性が高まる。

本発明の更に他の目的、本発明によって得られる利点は、以下において図面を参照して説明される実施に形態から一層明らかにされるであろう。図面の簡単な説明

[図 1]図 1Aは、階段を撮影した画像を示す模式図、図 1Bは、図 1Aから取得した 3次元距離データから 4つの平面領域 A、 B、 C、 Dを検出した結果を示す図である。

[図 2]図 2は、 under- segmentationを説明するための模式図である。

[図 3]図 3A乃至図 3Dは、ハフ変換により平面を抽出する方法を説明する図であり、図 3Aは、階段を示す図、図 3Bは、図 3Aに示す階段力得られた 3次元距離データを示す図、図 3Cは、図 3Bの距離データをノヽフ変換して求めたヒストグラムを示す図、図 3Dは、図 3Cに示すピークが示す平面と実際の平面との比較結果を示す図である

[図 4]図 4は、線分拡張法による平面検出処理を示すフローチャートである。

[図 5]図 5は、従来の線分を抽出する処理を説明する図であって、図 5A乃至図 5Cは

、その処理工程順に示す図である。

[図 6]図 6A及び図 6Bは、それぞれノイズが少な、計測データ点群及びノイズが多!ヽ計測データ点群に対し、 2つの閾値設定をした場合における線分抽出処理の結果の相違を示す図である。

[図 7]図 7は、本発明の実施の形態におけるロボット装置の概観を示す斜視図である

[図 8]図 8は、上記ロボット装置が具備する関節自由度構成を模式的に示す図である

[図 9]図 9は、上記ロボット装置の制御システム構成を示す模式図である。

[図 10]図 10は、本実施の形態における平面検出装置を示す機能ブロック図である。

[図 11]図 11は、上記ロボット装置が外界を撮影して、る様子を示す模式図である。

[図 12]図 12は、階段を示す模式図であって、図 12Aは、階段を正面から見た図、図

12Bは、階段を側面から見た図、図 12Cは、階段を斜めから見た図である。

[図 13]図 13は、階段の他の例を示す模式図であって、図 13Aは、階段を正面から見た図、図 13Bは、階段を側面から見た図、図 13Cは、階段を斜め力も見た図である。

[図 14]図 14Aは、図 13に示す階段をステレオビジョンシステムによって前方力も撮影した場合の画像を示す模式図、図 14B乃至図 14Dは、図 14Aに示す画像から取得した 3次元の距離データを示す図である。

[図 15]図 15Aは、図 13に示す階段をステレオビジョンシステムによって側方力も撮影した場合の画像を示す模式図、図 15B乃至図 15Dは、図 15Aに示す画像から取得した 3次元の距離データを示す図である。

[図 16]図 16Aは、図 13に示す階段をステレオビジョンシステムによって斜め前方から撮影した場合の画像を示す模式図、図 16B乃至図 16Dは、図 16Aに示す画像から取得した 3次元の距離データを示す図である。

[図 17]図 17は、テクスチャを付与する手段を有しているロボット装置を説明するための図である。

[図 18]図 18は、本実施の形態における線分拡張法による平面検出方法を説明する図である。

[図 19]図 19は、線分拡張法による平面検出処理を示すフローチャートである。

[図 20]図 20は、本実施の形態における線分抽出部における処理の詳細を示すフロ一チャートである。

[図 21]図 21は、距離データ点の分布の様子を示す図であって、図 21 Aは、データの分布が線分に対してジグザグ形である場合、図 21Bは、ノイズなどにより線分近傍に一様に分布して、る場合を示す模式図である。

[図 22]図 22は、本実施の形態における Zig-Zag-Shape判別方法を示すフローチヤ一トである。

[図 23]図 23は、上記 Zig-Zag-Shape判別処理を示す図である。

[図 24]図 24は、 Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。

[図 25]図 25は、本実施の形態における領域拡張処理を説明するための模式図である。

[図 26]図 26は、本実施の形態における領域拡張部における領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。

[図 27]図 27は、端点と直線との距離が等しくても平面方程式の 2乗平均誤差 rmsが異なる例を示す図であって、図 27Aは、ノイズなどの影響により線分が平面力ずれている場合、図 27Bは、線分が属する他の平面が存在する場合を示す模式図である【図 2 8】図 2 8は、領域種の選択処理を示す図である。

【図 2 9】図 2 9は、領域拡張処理を示す図である。

【図 3 0】図 3 0 Aは、ロポット装置が立った状態で床面を見下ろした際の床面を示す模式図、図 3 0 Bは、縦軸を x、横軸を y、各デ一夕点の濃淡で z軸を表現して 3次元距離データ及び、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す図、図 3 0 Cは、図 3 0 Bに示す直線群から領域拡張処理により得られた平面領域を示す図である。

【図 3 1】図 3 1は、床面に段差を一段置いたときの本実施の形態における平面検出方法と従来の平面検出方法との結果の違いを説明す βる· 2ための図であって、図 3 1 Aは、観察された画像を示す模式図、図 3 1 Bは、実験条件を示す図、図 3 1 Cは、本実施の形態における平面検出方法により平面検出された結果を示す図、図 3 1 Dは、従来の平面検出方法により平面検出された結果を示す図である。

【図 3 2】図 3 2 Aは、床面を撮影した画像を示す模式図、図 3 2 B及び図 3 2 Cは、図 3 2 Aに示す床面を撮影して取得した 3次元距離データから水平方向及び垂直方向の距離データ点列から、それぞれ本実施の形態の線分検出により検出した線分及び従来の線分検出により検出した線分を示す図である。

【図 3 3】図 3 3 Aは、階段を撮影した画像を示す模式図、図 3 3 B乃至図 3 3 Dは、図 '3 3 Aから取得した 3次元距離デ一夕を使用して、それぞれ上面、正面、側面から平面を検出した例を示す図である。

【図 3 4】図 3 4 Aは、他の階段を撮影した画像を示す模式図、図 3 4 B乃至図 3 4 Dは、図 3 4 Aから取得した 3次元距離データを使用して、それぞれ上面、正面、側面から平面を検出した例を示す図である。

発明を実施するための最良の形態

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、複数平面を同時に精度よく検出可能な平面検出装置を搭載したロポット装置に適用したものである。

本実施の形態における平面検出装置は、ステレオビジョンなどにより得られた距離情報

(距離データ）から複数の平面を抽出することができ、計測ノイズに対して口パス

差撬: トな検出を行うことが可能であり、例えば、ロボット装置に搭載すれば、ロボット装置が自身の周囲の環境を正確に認識することができ、例えば階段などの複数平面を含む物体、周囲の状況を認識し、この認識結果に応じて自律的に移動したり、行動したりすることができる。

本実施の形態においては、まず、このようなロボット装置の一例として 2足歩行タイプのロボット装置を例にとって説明する。このロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態 (怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボット装置である。なお、ここでは、 2足歩行型のロボット装置を例にとって説明するが、 2足歩行のロボット装置に限らず、 4足又は車輪等により移動可能なロボット装置に適用できることはいうまでもない。

図 7は、本実施の形態におけるロボット装置の概観を示す斜視図である。図 7に示すように、ロボット装置 201は、体幹部ユニット 202の所定の位置に頭部ユニット 203 が連結されるとともに、左右 2つの腕部ユニット 204RZLと、左右 2つの脚部ユニット 205RZLが連結されて構成されている（但し、 R及び Lの各々は、右及び左の各々を示す接尾辞である。以下において同じ。 ) o

このロボット装置 201が具備する関節自由度構成を図 8に模式的に示す。頭部ュ- ット 203を支持する首関節は、首関節ョー軸 101と、首関節ピッチ軸 102と、首関節口一ノレ軸 103と!ヽぅ 3自由度を有して！/ヽる。

また、上肢を構成する各々の腕部ユニット 204RZLは、肩関節ピッチ軸 107と、肩関節ローノレ軸 108と、上腕ョー軸 109と、月寸関節ピッチ軸 110と、前腕ョー軸 111と、手首関節ピッチ軸 112と、手首関節ロール輪 113と、手部 114とで構成される。手部 114は、実際には、複数本の指を含む多関節 *多自由度構造体である。ただし、手部 114の動作は、ロボット装置 201の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は 7 自由度を有するとする。

また、体幹部ユニット 202は、体幹ピッチ軸 104と、体幹ロール軸 105と、体幹ョー軸 106という 3自由度を有する。また、下肢を構成する各々の脚部ユニット 205RZLは、股関節ョー軸 115と、股関節ピッチ軸 116と、股関節ロール軸 117と、膝関節ピッチ軸 118と、足首関節ピッチ軸 119と、足首関節ロール軸 120と、足底 121とで構成される。本明細書中では、股関節ピッチ軸 116と股関節ロール軸 117の交点は、ロボット装置 201の股関節位置を定義する。人体の足底 121は、実際には多関節，多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置 201の足底は、ゼロ自由度とする。したがって、各脚部は、 6自由度で構成される。

以上を総括すれば、ロボット装置 201全体としては、合計で 3 + 7 X 2 + 3 + 6 X 2 = 32自由度を有することになる。ただし、エンターテインメント向けのロボット装置 201が必ずしも 32自由度に限定されるわけではない。設計'制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。

上述したようなロボット装置 201がもつ各自由度は、実際にはァクチユエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、 2 足歩行と！/、う不安定構造体に対して姿勢制御を行うこと等の要請から、ァクチユエ一タは小型且つ軽量であることが好ま U、。

このようなロボット装置は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット 202等に備える。図 9は、ロボット装置 201の制御システム構成を示す模式図である。図 9に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール 200と、ァクチユエータ 350の駆動等、ロボット装置 201の全身協調運動を制御する運動制御モジュール 300とで構成される。

思考制御モジュール 200は、情緒判断や感情表現に関する演算処理を実行する C PU (Central Processing Unit) 211や、 RAM (Random Access Memory) 212、 ROM (Read Only Memory) 213及び外部記憶装置（ノヽード ·ディスク ·ドライブ等） 214等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。

この思考制御モジュール 200は、画像入力装置 251から入力される画像データや音声入力装置 252から入力される音声データ等、外界からの刺激等に従って、ロボット装置 201の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データ力ユーザの表情を認識し、その情報をロボット装置 201の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置 251は、例えば CCD (Charge Coupled Device)カメラを複数備えており、これらのカメラにより撮像した画像力も距離画像を得ることができる。また、音声入力装置 252は、例えばマイクロホンを複数備えている。

思考制御モジュール 200は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール 300に対して指令を発行する。

一方の運動制御モジュール 300は、ロボット装置 201の全身協調運動を制御する CPU311や、 RAM312、 ROM313及び外部記憶装置（ノヽード'ディスク'ドライブ等 ) 314等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置 314には、例えば、オフラインで算出された歩行パターンや目標とする ZMP軌道、その他の行動計画を蓄積することができる。

この運動制御モジュール 300には、図 8に示したロボット装置 201の全身に分散するそれぞれの関節自由度を実現するァクチユエータ 350、対象物との距離を測定する距離計測センサ（図示せず)、体幹部ユニット 202の姿勢や傾斜を計測する姿勢センサ 351、左右の足底の離床又は着床を検出する接地確認センサ 352, 353、足底 121の足底 121に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置 354等の各種の装置力バス 'インタフェース（IZF) 310経由で接続されている。ここで、姿勢センサ 351は、例えば加速度センサとジャイロ 'センサの組み合わせによつて構成され、接地確認センサ 352, 353は、近接センサ又はマイクロ 'スィッチ等で構成される。

思考制御モジュール 200と運動制御モジュール 300は、共通のプラットフォーム上で構築され、両者間はバス'インタフェース 210, 310を介して相互接続されている。運動制御モジュール 300では、思考制御モジュール 200から指示された行動を体現すベぐ各ァクチユエータ 350による全身協調運動を制御する。すなわち、 CPU3 11は、思考制御モジュール 200から指示された行動に応じた動作パターンを外部記憶装置 314から取り出し、又は、内部的に動作パターンを生成する。そして、 CPU3 11は、指定された動作パターンに従って、足部運動、 ZMP軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各ァクチユエータ 350に転送する。

また、 CPU311は、姿勢センサ 351の出力信号によりロボット装置 201の体幹部ュニット 202の姿勢や傾きを検出するとともに、各接地確認センサ 352, 353の出力信号により各脚部ユニット 205RZLが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット装置 201の全身協調運動を適応的に制御することができる。更に、 CPU311は、 ZMP位置が常に ZMP安定領域の中心に向力うように、ロボット装置 201の姿勢や動作を制御する。

また、運動制御モジュール 300は、思考制御モジュール 200において決定された意思通りの行動がどの程度発現された力すなわち処理の状況を、思考制御モジュール 200〖こ返すよう〖こなっている。このようにしてロボット装置 201は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。

このようなロボット装置においては、頭部ユニット 203にステレオビジョンシステムを搭載し、外界の 3次元距離情報を取得することができる。次に、このようなロボット装置などに好適に搭載されるものであって、ステレオビジョンによる 3次元距離情報を利用した本実施の形態における平面検出装置について説明する。なお、距離情報としては、レーザレンジファインダ (レーザ距離計測計)などによる距離情報を使用してもよいことは勿論である。

本実施の形態における平面検出装置は、線分拡張法により、視野内において支配的な平面だけでなぐ例えば階段など複数の平面が存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさせることにより計測ノィズに対してロバストな平面検出結果を得ることができるものである。

図 10は、本実施の形態における平面検出装置を示す機能ブロック図である。図 10 に示すように、平面検出装置 1は、 3次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System) 2と、 3次元の距離データからなる距離画像に存在する平面を線分拡張法により検出する平面検出部 3とを有する。平面検出部 3は、画像を構成する距離データ点から同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部 4と、画像内に含まれる、線分抽出部 4よって抽出された全線分力ゝらなる線分群から、該画像内に存在する 1又は複数の平面領域を検出する領域拡張部 5とを有する。領域拡張部 5は、線分群力同一平面上に存在すると推定される任意の 3本の線分を選択し、これらから基準平面を求める。そして、選択した 3本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新するとともに基準平面の領域を拡張する。

ステレオビジョンシステム 2は、例えばロボット装置 201の画像入力装置 251により取得された画像から距離画像を生成するものであり、外界を観測した結果、両眼の視差によって推定される 3次元距離データ D1を線分抽出部 4に出力する。

線分抽出部 4は、その距離画像における列または行毎の各データ列において、 3 次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて 1以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群 D2を領域拡張部 5に出力する。

領域拡張部 5は、この線分群 D2において、同一の平面に属すると推定される線分を 3本選択し、これら力基準平面としての種となる平面を求める。この種となる平面の領域 (領域種: seed region)に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群 D3を出力する。ロボット装置 201は、障害物回避や階段昇降など平面の情報が必要なとき、または定期的にこれらの処理を行うことによって、階段や床面、壁といった歩行に重要な平面の情報を取得する。

次に、平面検出装置 1を構成するステレオビジョンシステム 2につヽて更に詳細に説明する。ステレオビジョンシステム 2は、人間の両眼に相当する左右 2つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、 3 次元距離情報を画像として出力 (距離画像)する。

図 11は、ロボット装置 201が外界を撮影している様子を示す模式図である。床面を X— y平面とし、高さ方向を z方向としたとき、図 11に示すように、画像入力部 (ステレオカメラ）を頭部ユニット 203に有するロボット装置 201の視野範囲は、ロボット装置 201 の前方の所定範囲となる。

ロボット装置 201は、上述した CPU211において、画像入力装置 251からのカラー画像及び視差画像と、各ァクチユエータ 350の全ての関節角度等のセンサデータとなどが入力されてソフトウェア構成を実現する。

本実施の形態のロボット装置 201におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。なお、ロボット装置の位置を示す座標として、例えば、ランドマーク等の特定の物体等に基づく所定位置を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心 (座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との 2つの座標を使用する。

ステレオビジョンシステム 2では、カラー画像及びステレオカメラによる視差画像などの画像データが撮像された時間において、センサデータ力も割り出した関節角を使用してロボット装置 201が中心に固定されたロボット中心座標系を頭部ユニット 203 に設けられた画像入力装置 251の座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列とこれに対応する 3次元距離データからなる距離画像を平面検出部 3へ出力する。ここで、本実施の形態における平面検出装置は、上述したノ、フ変換のように、取得した画像に含まれる支配的な平面を検出するのみならず、階段など複数平面が含まれている場合であっても平面を検出可能とするため、線分拡張法により平面を検出する。そして、この際、距離データ点の分布に応じて線分を生成することにより、計測ノイズに対してロバストな検出結果を得ることができるものである。以下では、本実施の形態における平面検出装置を搭載したロボット装置が視野内に含まれる階段 STを検出する場合を例にとって説明する。

階段としては、例えば図 12、図 13に示すようなものがある。図 12A、図 13Aは、階段を正面から見た図、図 12B)、図 13Bは、階段を側面から見た図、図 12C、図 13C は、階段を斜め力も見た図である。

ここで、本明細書においては、人間、ロボット装置などが階段を昇降するために使用する面 (足又は可動脚部を載せる面）を踏面と、、、一の踏面力その次の踏面までの高さ（1段の階段の高さ）を蹴り上げということとする。また、階段は、地面に近い方から 1段目、 2段目とカウントすることとする。

図 12に示す階段 ST1は、段数が 3段の階段であり、蹴り上げ 4cm、 1, 2段面の踏面の大きさは幅 30cm、奥行き 10cm、最上段である 3段目の踏面のみ、幅 30cm、奥行き 21cmとなっている。また、図 13に示す階段 ST2も段数が 3段の階段であり、蹴り上げ 3cm、 1, 2段面の踏面の大きさは幅 33cm、奥行き 12cm、最上段である 3 段目の踏面のみ、幅 33cm、奥行き 32cmとなっている。

図 14乃至図 16は、図 13に示す階段 ST2を示し、図 14A、図 15A、図 16Aは、図 13に示す階段をステレオビジョンシステムによってそれぞれ前方、側方、斜め前方から撮影した場合の画像を示す模式図、図 14B乃至図 16Dは、図 14A、図 15A、図 1 6Aに示す画像から取得した 3次元の距離データを示す図である。

図 14Aに示すように、正面から階段 ST2を撮影した場合、 3次元の距離データは、図 14B乃至図 14Dのようになる。図 14Bにおいて、横軸を y方向、縦軸を x方向とし、 z軸方向（高さ方向）の大きさは、ロボット装置 201の接地面を 0とし、高さが高くなるほど白に近づくような濃淡値で示している。すなわち、濃淡 (濃淡値）が同様のデータ点は同一高さにあることを示し、図 14Bに示すように、階段 ST2においても 1段目より 2 段目、 2段目より 3段目の踏面にあたる領域のデータ点の濃淡が薄くなつている。また、距離データが図示されている略台形領域がロボット装置が撮影可能な範囲 (視野範囲）を示す。また、同図において、距離データ点は略 4段階の濃淡に分かれているが、 z方向が最も小さい領域に対応する最も濃淡が濃い箇所は、床面を示している。また、図 14Cは、横軸を y方向、縦軸を z方向、 X方向を色の濃淡で示すものである。この図では、 X方向の距離が大きくなるほど濃淡が薄くなるように表現されている。図 14Dは、横軸を X方向、縦軸を z方向とし、 y方向をその距離に応じて濃淡で表現している。

また、ロボット装置 201が階段 ST2の側面を撮影した場合は、図 15A乃至図 15D に示すように、 X軸が大きい上方側の領域に存在するデータ点が、高さが 0と同様の濃淡を示しており、階段 ST2の奥の床面を計測した結果であることを示している。また、図 16A乃至図 16Dに示す斜め方向からの撮像においても、床面及び 1一 3段目の踏面を示す 4つの領域が、高さの違いに応じて異なる濃淡で示され、はっきり区別できて、ることを示して！/、る。

ここで、このような 3次元距離データをステレオカメラによって取得するためには、階段 ST2の表面に模様 (テクスチャ）が必要となる。すなわち、 2台のカメラによる視差により得ることができるため、模様がないものは視差が算出できず、正確に距離を計測することができない。すなわち、ステレオビジョンシステムにおける距離データの計測精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。

そこで、図 17に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステムを構成するステレオカメラ 11RZLを備えるとともに、例えば同じく頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源 12を設ける。この光源 12は、模様がない階段 ST3、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射 (照射)し、ランダムなパターン PTを付与する模様付与手段として作用する。なお、ランダムパターン PTを形成して距離画像を取得できるものであれば、ランダムパターン PTを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載される CCDカメラなどにおいては観測可能なパターンを付与することができる。

次に、平面検出装置 1の平面検出部 3について説明する。この平面検出部 3は、線分拡張法を使用して平面を検出するものであり、図 18は、線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出では、図 18に示すように、まず、焦点 Fから撮影された画像 11において、行方向または列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row:イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分力もなる線分群にぉ、て、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。

図 19は、線分拡張法による平面検出処理を示すフローチャートである。図 19に示すように、先ず、距離画像を入力し (ステップ S 1)、距離画像の行方向（又は列方向）の各画素列において同一平面に属すると推定されるデータ点力も線分を求める (ステツプ S2)。そして、これらの線分群の中から同一平面に属すると推定される線分を抽出し、これらの線分力もなる平面を求める（ステップ S3)。このステップ S3では、まず、平面の種となる領域 (以下、領域種 (seed region)という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の 1ラインを含む 3本の線分が同一平面にあることを条件とする。ここで、選択した 3本の線分からなる領域種が属する平面を基準平面とし、 3本の線分から平均して求まる平面を求めておく。また、 3本の線分からなる領域を基準平面領域とする。

そして、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し (領域拡張処理)、追加した線分を含めたものとして上記基準平面を更新し (平面更新処理)、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域 (3 本の線分)が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結する。そして、本実施の形態においては、得られた平面に属する線分群のうち、平面力所定の閾値以上外れる線分を除、て再度平面を求める平面再算出処理をステップ S4として更に設け、最終的な平面とする力詳細は後述する。

ここで、 3次元距離データから線分を検出し、これを同一平面毎にまとめた領域を 1 つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本実施の形態においては、ステップ S2における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しょうとしても、距離データの精度に応じて閾値を変更しなければ、 over— segmentation又は under— segmentationなどもの問題か生じてしまつ。そこで、本実施の形態においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。

以下、図 19に示す線分拡張法による平面検出方法について更に詳細に説明する。線分抽出器 (Line Extraction)4は、上述したように、ステレオビジョンシステム 2からの 3次元距離画像を入力とし、距離画像の各列または各行毎に 3次元空間内で同一平面上にあると推定される線分を検出する。この線分抽出において、計測ノイズなどによる、上し 7こ over— segmentationや under— segmentationのロ題、すなわ、本来複数の平面であるのに 1つの平面として認識してしまつたり、本来は 1つの平面であるのに、複数の平面として認識してしまったりする問題を回避するため、データ点の分布に応じて適応的に線分フィッティングさせるアルゴリズム（Adaptive Line Fitting)を導入する。 Adaptive Line Fittingは、線分抽出部 4において、先ず比較的大きい閾値を使用して大まかに第 1の線分としての線分を抽出し、次に抽出された第 1の線分に属するデータ点群から後述する最小二乗法によって得られる第 2の線分としての線分に対する該データ点群の分布を解析する。すなわち、同一平面上に存在するか否かを大まかに推定してデータ点群を抽出し、抽出したデータ点群におけるデータ点の分布の偏りがある力否かを解析して同一平面上に存在している力否かを再度推定する。

本実施の形態においては、このデータ点の分布を解析し、データ点群が後述するジグザグ形 (zig-zag-shape)に当てはまる場合には、分布に偏りがあるとしてデータ点群を分割する処理を行い、これを繰り返すことによって、データ点群に含まれるノイズに対して適応的に線分の抽出を行うアルゴリズムを使用するものとする。

図 20は、線分抽出部 4における処理、すなわち、図 19におけるステップ S2の処理の詳細を示すフローチャートである。まず、線分抽出部 4には、距離データが入力される。入力された距離データのうち、例えば行方向の画素列（データ点列）において、 3次元空間上で同一平面上に存在すると推定されるデータ点群を抽出する。 3次元空間上で同一平面上に存在すると推定されるデータ点群は、例えば隣接するデータ点の距離が、例えば 6cm以下など、データ点間の 3次元空間における距離が所定の閾値以下のものからなるデータ点の集合などとすることができ、これをデータ点群 (P[ 0· · · n-1 ])として抽出する (ステップ SI 1)。そして、このデータ点群 Ρ[0· · ·η-1]に含まれるサンプル数 ηが処理に最低限必要なサンプル数 (必要最小値) min_nより多ヽか否かをチェックし (ステップ S 12)、データ数 nが必要最小値 min_nより少ない場合（S 2 : YES)には、検出結果として空集合を出力して処理を終了する。

一方、サンプル数 nが必要最小値 min_n以上である場合（S2 :NO)、データ点群 P[ 0· · ·η-1]の一方の端点 Ρ [0]と他方の端点 P [n-1]とを結ぶ線分 (弦) L 1を第 1の線分として生成する。そして、データ点群 Ρ[0· · ·η-1]から、この線分 L1との距離が最も大き、データ点を着目点 brkとして検索し、その距離 distを算出する (ステップ S 13)。最大距離 distがデータ点群分割の閾値 ma_X_dより大きい場合には（S14 :YES)、デ一タ点群データ点群 Ρ[0· · ·η-1]を着目点（分割点） brkにて 2つのデータ点群 ρ [0· · •brk]及び P[brk' · · n-1]に分割する（ステップ S 18)。

一方、最大距離 distがデータ点群分割の閾値 max_dより小さい場合には（S 14 : NO )、データ点群 Ρ[0· · ·η-1]力後述する最小二乗法によって最適な線分の方程式 lineを求め（ステップ S 15)、この方程式 lineが示す線分 L2を第 2の線分として生成する。そして、データ点群 Ρ[0· · ·η-1]がこの線分 L2に対して後述する Zig-Zag-Shape であるかどうかを調べる（ステップ S 16)、 Zig- Zag- Shapeでない場合（S16 : NO)、得られた線分の方程式 lineを線分抽出結果リストに追加し (ステップ SI 7)、処理を終了する。

また、ステップ S 16にお!/、てステップ S 15で求めた線分が Zig-Zag-Shapeである判断された場合（S 16: YES)の場合、上述のステップ S 14と同様、ステップ S 18に進み、ステップ S 13において距離 distを求めた着目点 brkにてデータ点群を 2つのデータ点群 Ρ[0· · 'brk]及び P[brk' · ·η- 1]に分割する。このステップ S 18にて 2つのデータ点群が得られた場合には、それぞれを再帰的に再度ステップ S11からの処理を行う。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すなわち全てのデータ点群がステップ S 17を経るまで処理を繰り返し、これにより、全ての線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群 Ρ [0· · ·η-1]からノイズの影響を排除し複数の線分力もなる線分群を精度よく検出することができる。

なお、ステップ S13にてデータ点群 Ρ[0· · ·η-1]の端点を結ぶ線分 L1を生成するものとして説明したが、例えばデータ点群 Ρ[0· · ·η-1]の分布、性質など必要に応じてデータ点群 Ρ[0· · ·η-1]力最小二乗により線分 L1を求めてもよい。また、本実施の形態においては、着目点 brkは、端点を結んだ線分 L1との距離が最大の点 1つとしているが、例えば、上記のように最小二乗により求めた線分との距離が最大の点としたり、距離がデータ点群分割の閾値 max_d以上のものが複数ある場合はそれら全ての点又は選択した 1つ以上にてデータ点群 Ρ[0· · ·η-1]を分割するようにしてもよい。次に、ステップ S15における最小二乗による線分生成方法（Least-Squares Line Fitting)について説明する。ある n個のデータ点群 Ρ[0· · ·η-1]が与えられたとき、データ点群に最もフィットした直線の方程式を求める方法を示す。直線の方程式のモデルを下記式（1)で表す。

[数 1] xcosa + y&in + d = 0 …ひ) この場合、 n個のデータ点群 Ρ[0· · ·η-1]の 1点（x,y)において、直線方程式のモデルとデータ点との誤差の総和は下記式（2)で表すことができる。 [数 2]

^Efit ⁼∑ ^cos ^ + y_£ sin ^ + ίί)² ---(2) データ点群に最もフィットした直線は、上記式（2)の誤差の総和を最小化することによって求められる。上記式（2)を最小にする α及び dは、データ点群 Pの平均及び分散共分散行列を用いて下記（3)のように求めることができる。

[数 3]

次に、ステップ S 16におけるジグザグ形 (Zig-Zag-Shape)判別方法につ!、て説明する。この Zig- Zag- Shape判別では、ある n個のデータ点群 Ρ[0· · ·η- 1]と直線 Line , d)、 xcosa +ycosa +d=0力与えられたとき、そのデータ点群 Ρ[0· · ·η- 1]が、図 2 1Aに示すように直線 Lineに対して交差する力、図 21Bに示すように、例えばノイズなどの影響によりデータ点が一様に分布しているかを判別するものである。基本的には

、直線 Lineの一方にデータ点群 Ρ[0· · ·η-1]が連続して現れる数をカウントし、ある一定数を超えて連続して現れる場合には、 zig-zag-shapeであると判断することができる。図 21Aの場合には、データ点群 Ρ[0···η-1]によりよくフィットする直線 Lineを求めるためにデータ点群 P[i]を分割する必要がある。図 22は、 Zig-Zag-Shape判別方法を示すフローチャートである。

まず、データ点群 Ρ[0···η-1]と直線 Line ,d, σ )とを入力する (ステップ S 20)。ここで、 σは、点列の標準偏差を示す。次に、この標準偏差 σが所定の閾値 th_ σより大きいか否かを判断する。この標準偏差 σが閾値 th_aより小さい場合 (ステップ S2 1:Νο)は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、判別を終了する。そして、標準偏差 σが閾値 th_ σより大き!/、場合のみ判別処理を継続する。次に、データ点群 Ρ [0 · · ·η-1]のうちの最初のデータ点 Ρ [0]が直線のどちら側にあるかを sing(sdist(P[0]》によって判断し、この結果を valに代入するとともに valと

0 0 同じ側にあるデータ点の連続数をカウントするカウンタ（以下、連続点カウンタといい

、このカウント値をカウント値 countという。）のカウント値 countを 1に設定する（ステップ S22)。ここで、 sign(x)は、 Xの値の符号（+又は一)を返す関数であり、 sdist(i)は、 P[i] .xcos a + P[i].ycos α +dとして計算された直線 Lineにおいて、 i番目のデータ点との正負の距離を示す。すなわち、 Valには、データ点 P [0]が直線 Lineのどちら側にあるか

0

で +又は一の符号が代入される。

次に、データ点をカウントするためのカウンタ（以下、データ点カウンタといい、このカウント値をカウント値 iという。）のカウント値 iを 1とする（ステップ S23)。そして、データ点カウンタのカウント値 iがデータ数 nより小さい場合 (ステップ S24 : YES)、その次のデータ（以下、 i番目とする。）のデータ点であるデータ点 P [i]が直線のどちら側にあるかを sing(sdist(P[i]》によって判断し、この結果を valに代入する (ステップ S25)。そして、ステップ S22にて求めた valとステップ S25にて求めた valとを比較し、 valと

0 0 valとが異なる場合 (ステップ S26 : NO)、 valに valを代入し、連続点カウンタのカウン

0

ト値 countに 1を代入し (ステップ S28)、データ点カウンタのカウント値 iをインクリメントして（ステップ S30)ステップ S 24からの処理に戻る。

一方、ステップ S26において、 valと valとが同じ場合 (ステップ S26 : YES)、データ

0

点 P [i— 1]と P [i]は、直線 Lineに対して同じ側にあると判断され、連続点カウンタの力ゥント値 countを 1つインクリメントする（ステップ S27)。さらに、連続点カウンタのカウント値 countが Zig-Zag-Shapeと判定されるための最小のデータ点数 min_cより大きいか否か判定し (ステップ S29)、大きい場合には（ステップ S29 : YES)、 Zig- Zag- Shapeと判断し、 TRUEを出力して処理を終了する。一方、連続点カウンタのカウント値 count が最小のデータ点数 min_cより小さい場合には（ステップ S29 : NO)、ステップ S30に進み、データ点カウンタのカウント値 iをインクリメントして (ステップ S30)、ステップ S2 4からの処理を繰り返す。

そして、このステップ S24からの処理を、データ点カウンタのカウント値 iがデータ点数 nに到達するまで続け、カウント値 i≥nとなったところで、 FALSEを出力して処理を終了する。

このようなジグザク形判別処理によって、 n個のデータ点群 Ρ [0 · · ·η-1]と直線 Line ( a ,d) : xcos a +ycos a + d=0が与えられたとき、このデータ点群が直線 Lineに対して zig-zagに交差しているかどうかを判断することができる。これによつて、上述したように、ステップ S 16にてデータ点群を分割するべき力どうかを判断することができ、最小二乗により求めた直線に対し、データ点群が zig-zagに交差していると判断した場合にはデータ点群を分割すべきと判断してステップ S 18の処理へ進み、着目点 brkを分割点としてデータ点群を分割することができる。なお、上記ステップ S21—ステツプ S30までの処理は図 23のように表現することも可能である。

また、このような Zig-Zag-Shape判別処理は、演算器のみならずノヽードウエアで行うことも可能である。図 24は、 Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。図 24に示すように、 Zig-Zag-Shape判別処理部 20は、 n個のデータ点群 P [0 • · ·η-1]が入力され、順次各データ点 P[i]が直線 Lineのいずれ側に位置するかを判別し、その判別結果 Valを出力する方向判別部 21と、 1つ後のデータと方向判別部 2 1の結果を比較させるための遅延部 22と、データ点 P[i]における方向判別結果 Valとデータ点 P[i— 1]における方向判別結果 Valとを比較する比較部 23と、比較部 23に

0

おいて Val=Valの場合に、カウント値をインクリメントする連続点カウンタ 24と、連続

0

点カウンタ 24のカウント値 countと最小データ点数格納部 26から読み出した最小データ点数 min_cとを比較する比較部 25とを有する。

この Zig-Zag-Shape判別処理部における動作は以下のようになる。すなわち、方向判別部 21は、データ点群 Ρ [0 · · ·η-1]力も最小二乗法により直線 Lineを求め、各データ点 P[i]と直線 Lineとの正負の距離を求め、その正負の符号を出力する。遅延部 2 2は、データ点 P[i— 1]の直線 Lineまでの距離に対する正負の符号が入力されると 1つ後のデータ点 P[i]の正負の符号が入力されるタイミングまでデータを格納する。

比較部 23は、データ点 P[i]とデータ点 P[i— 1]の上記正負の符号を比較し、同じ符号である場合には接続点カウンタ 24のカウント値 countをインクリメントする信号を出力し、正負の符号が異なればカウント値 countに 1を代入する信号を出力する。比較部 25は、カウント値 countと最小データ点数 min_cとを比較し、最小データ点数 min_cよりカウント値 countが大きい場合には、データ点群 Ρ [0 · · ·η-1]がジグザグであることを示す信号を出力する。

次に、図 10に示す領域拡張部（Region Growing) 5について説明する。領域拡張部 5は、線分抽出部 4によって得られた線分群を入力とし、それらの線分それぞれがどの平面に属しているかを点列の平面への当てはめ（Plane Fitting)により判断し、与えられる線分群からなる領域を複数の平面 (平面領域）に分離する。複数の平面に分離するために、以下の手法をとる。

先ず、与えられた線分群から、同じ平面上にあると推定される隣接する 3本の線分を検索する。この 3本の線分により求められる平面 (基準平面）平面の種となるものであり、この 3本の線分が含まれる領域を領域種（seed region)という。そして、この領域種に隣接する線分を順次、基準平面と同一平面上にある線分か否かを点列の平面への当てはめ（Plane Fitting)により判断し、隣接する線分が同じ平面に含まれると判断された場合には、この線分を領域拡大用の線分として領域種に追加してその領域を拡大するとともに、基準平面の方程式を上記領域拡大用の線分を含めて再度算出し直す。このような処理によって、全ての線分を何れかの領域 (平面）に配分する図 25は、領域拡張処理を説明するための模式図である。図 25に示すように、画像 30内に複数の平面力もなる階段 31が存在する場合、例えば太線で示す 32a— 32c の 3本の線分が領域種として選択されたとする。これら 3本の線分 32a— 32cからなる領域が領域種となる。先ず、この 3つの線分 32a— 32cにより 1つの平面 (基準平面） Pを求める。次に、領域種の最も外側の線分 32a又は 32cに領域種外にて隣接するそれぞれデータ列 33又は 34において、平面 Pと同一の平面である線分を選択する。ここでは、線分 33aが選択されるとする。次に、これら 4本の線分群からなる平面 P'を求め、基準平面 Pを更新する。次に、線分 34aが選択されれば、 5本の線分群からなる平面 P' 'を求め、平面 P'を更新する。これを繰り返すことにより、階段 31の 2段目の踏面が、破線で囲まれる平面 45として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像 30内から領域種となる 3つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる 3つの線分がなくなるまで

図 19のステップ S3の処理を繰り返す。

次に、データ点群 Ρ[0· · ·η-1]力も構成される平面の方程式を推定する手法 (Plane

Fitting)、これを使用して領域種を選択する方法（Selection of seed region)、領域種から領域を拡大していく領域拡張処理 (Region growing)、及び得られた平面方程式力も誤差が大きいものなどを除いて再度算出する後処理 (Post processing)について説明する。

3次元空間内の点 Pは P=(x, y, により表され、平面の方程式はその法線べタトル n(nx, ny, nz)と非負の定数 dによって下記式 (4)で表される。

画 xⁿx ⁺ yn_y+zn_z +d = 0 ".(4) ここで、図 11に示すように、ステレオカメラでは、焦点を通る平面を観測することが

できない、すなわち、平面は焦点を通らないため、 d≠0とすることができる。したがつて、平面は、最小二乗法により下記式（5)に示す値を最小にする値として求めることができる。

[数 5] (n，め =∑(p『n +め²' —(5) 最適解は n=m/ " m " , d=~l/■ m■として求まる。ここで、 ■ ·■は、ベクトルの大きさ、 mは、行列式によって連立一次方程式を解くクラメールの法則（Cramer's rule)を使

用して下記（6-1)のように容易に得られる線形システムの解である。

[数 6]

A m = b -- (6-1)

ここで、

^{A =}ム Ρ,Ρί , = _Pi 〜(6-2)

i i この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であつても、上記式 (6— 2)に示す Aと bの値を更新するのみで、平面パラメータを再計算することができる。更に、本実施の形態における線分抽出方法の場合は n個のデータ点群の 2つのモーメント（1次モーメント：平均、 2次モーメント：分散） E(p)、E(pp )が

T

既知であり、これらを使用して、下記（7)に示すように A, bを更新することができ、 n個のデータ点群における平面更新処理に拡張することができる。

[数 7]

Λ τ-Α + ηΕ(ρρ^τ) , b <- b + n£(p) .-.(7) また、一度平面パラメータ n, dを算出すれば、求まった平面方程式から、 n個のデータ点群の平面方程式からの外れ度合!、を示す平面方程式の 2乗平均誤差 (RMS (root mean square) residual) (以下、 rmsという。）を下記式（8)により算出することができる。この場合も、 n個のデータ点の上記 2つのモーメントを使用して下記式 (8)を求めることができる。

[数 8]

上記（8)に示すように、各データ点が求めた平面上にあれば平面方程式の 2乗平均誤差 rms(p ·'·ρ )は 0になる値であり、この値が小さいほど各データ点が平面によくフィットして！/、ることを示す。

次に、領域種 (seed region)を検索する方法及び領域種から領域を拡大すると供に平面を更新する方法について説明する。図 26は、領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。図 26に示すように、領域種の選択には、先ず、線分抽出の際に使用した行方向又は列方向のデータ列が隣接する 3つの線分 (1 , 1 , 1 )であって、互いの線分 (1 , 1 ), (1 , 1 )における画素位置が上記デー

1 2 3 1 2 2 3

タ列とは直交する方向にて重複したものを検索する (ステップ S31)。各データ点は画像内における画素位置を示すインデックス (index)を有しており、例えば行方向のデータ列における線分である場合、このインデックスを比較して列方向にて重複してヽるか否かを比較する。この検索に成功した場合 (ステップ S32: YES)、上記式（7)を使用して上記（6—1)を算出する。これにより、平面パラメータ n, dを決定でき、これを使用して上記式 (8)に示す平面方程式の 2乗平均誤差 (1 , 1 , 1 )を計算する (ステツ

1 2 3

プ S33)。そして、この平面方程式の 2乗平均誤差 rms (l， 1， 1 )が例えば lcmなど

1 2 3

の所定の閾値 th 1より小さい場合には、この 3つの線分を領域種として選択する (ス rms

テツプ S34)。所定の閾値 th 1より大きい場合には、再びステップ S31に戻り、上記 rms

条件を満たす線分を検索する。また、領域種に選ばれた線分は、線分群のリストから除くことで、他の平面拡張などの際に使用されないようにしておく。

こうして選択された領域種から線分拡張法により領域を拡張する。すなわち、先ず、領域種の領域に追加する候補となる線分を検索する (ステップ S35)。なお、この領域は、領域種が既に更新されている場合の、後述する更新された領域種も含む。候補となる線分は、領域種の領域に含まれる線分 (例えば 1 )

1 に隣接する線分 (1 )

4であつて、上述同様、これらの線分の画素位置が相互に重なりあうことを条件とする。検索が成功した場合 (ステップ S36 : YES)、その平面方程式の 2乗平均誤差 rms (1 )を

4 算出し、これが所定の閾値 th 2より小さいか否かを判定し (ステップ S37)、小さい rms

場合には平面パラメータを更新し (ステップ S38)、再びステップ S35からの処理を繰り返す。ここで、候補となる線分がなくなるまで処理を繰り返し、候補となる線分がなくなったら (ステップ S36 :NO)、ステップ S31の処理に戻り、再び領域種を検索する。そして、線分群に含まれる領域種がなくなった場合 (ステップ S32 : NO)、今まで得られて、る平面パラメータを出力して処理を終了する。

ここで、本実施の形態においては、領域種を検索し、 3つの線分が同一平面に属する力否かの判定、及び領域拡張処理を行う際に基準平面又はこれを更新した更新平面に属する力否かの判定には、上記式 (8)を使用する。すなわち、平面方程式の 2 乗平均誤差 rmsが所定の閾値 (th 1)未満である場合にのみその線分 (群）を同一 rms

平面に属するものと推定し、その線分を含めた平面として再び平面を算出する。このように平面方程式の 2乗平均誤差 rmsを使用して同一平面に属するか否かを判定することにより、更にノイズにロバストでかつ、細かい段差を含んでいるような場合にも正確に平面を抽出することができる。以下にその理由について説明する。

図 27は、その効果を示す図であって、端点と直線との距離が等しくても平面方程式の 2乗平均誤差 rmsが異なる例を示す模式図である。ここで、非特許文献 4のように、領域拡張処理する際、注目の直線 (線分)の端点 (end point)と平面 Pとの距離 Dの値が所定の閾値より小さい場合に、当該注目の線分が平面 Pと同一平面であるとして領域拡張処理を行うと、平面 Pに交差する直線 La (図 27A)と、平面 Pと平行で所定距離ずれているような直線 Lb (図 27B)とが同様に平面 Pの更新に使用されることとなる。ここで、平面方程式の 2乗平均誤差 rmsを求めると、図 27Aの直線 Laから求まる平面方程式の 2乗平均誤差 rms (La)に比して図 27Bの直線 Lbから求まる平面方程式の 2乗平均誤差 rms (Lb)の方が大きい。すなわち、図 27Aのように、直線 Laと平面 Pとが交差する場合は、平面方程式の 2乗平均誤差 rmsが比較的小さくノイズの影響である場合が多いのに対し、図 27Bのような場合、平面方程式の 2乗平均誤差 rm sが大きぐ直線 Lbは平面 Pと同一平面ではなく異なる平面 P'である確率が高い。したがって、複数の平面が含まれるような環境力平面を精確に求める必要がある場合などにおいては、本実施の形態のように、平面方程式の 2乗平均誤差 rmsを算出し、この値が所定の閾値 (th 2)未満である場合に同一平面と判断することが好まヽ rms なお、環境や距離データの性質に応じて、従来と同様、線分の端点と平面との距離が所定の閾値以下の場合は当該線分を平面に含めるようにしたり、これらを組み合わせてもよい。

上記の領域拡張における線分追加検定において利用される閾値 (th 2)は、実験 rms 等によって獲得された定数を用いることも可能だが、閾値の不適切な設定による Under- Segmentation, Over- Segmentationの問題が生じる為、線分に含まれる距離データの性質によって適応的に決定されることが望ましい。ノイズを多く含む線分群に対して低い閾値を設定すると、多くの線分が異なる領域に分割され、領域拡張が適切に行われない。

そこで本実施の形態では、下記式（9)により閾値 (th 2)を線分に含まれるノイズ rms

に応じて変化させることで、線分データの精度に応じた適応的な判定を可能とした。

th 2 = max(th 3, d sigmaQ )) - -- (9)

rms rms maha 2

ここで、 th 3は閾値 (th 2)の下限を定義する定数、 d はマハラノビス距離、 sigmaOは線分の分散を表す。ノイズを多く含むデータは、線分の分散 sigmaOが大きく、閾値 (th 2)が大きくなり、領域拡張に関する許容範囲が大きくなる。

rms

ここでは、 sigmaOとして上記式（2)で表される直線方程式とデータ点の誤差の総和 E を利用し、閾値の下限 th 3は領域種の検定で利用した線分の許容誤差閾値 th fit rms

1と同じとした。

rms

また、面パラメータ n, dを一旦算出すれば、平面方程式の 2乗平均誤差 rmsは、データ点群について線分抽出の際に求めた 2つのモーメントの値から平面方程式を更新し、上記式 (8)にて簡単に算出することができる。

また、上述の領域種の選択方法は、図 28のようにも表現することができる。 overlapQ , 1 )は、各イメージロウに含まれる直線ベクトル 1と 1における端点間の位置が直線べ j k j k

タトルとは直交する位置にて重なっている場合に trueを出力する関数である。また、 fitPlaned , 1 , 1 )は、上記式（4)一（7)により Am=bの解を求め平面パラメータ n, dを

1 2 3

計算し、上記式 (8)により算出された A, b〖こより、直線ベクトル 1 , 1 , 1を平面にフイツ

1 2 3

ティングさせる関数である。

rms(l , 1 , 1 )は、上記式（6)を使用して 3本の直線全てにおいて、平面方程式の 2

1 2 3

乗平均誤差 rmsの値を算出する関数である。また、 removed , 1 , 1 )は、 lines [i] , lines

1 2 3

[i+l]l , lines [i+2]から領域種を構成するとして選択されたそれぞれ直線 1 , 1 , 1を除

2 1 2 3 くことを意味し、これにより、再びこれらの直線が計算に使用されることを防止する。また、領域拡張処理は、図 29のように表現することもできる。図 27において、 A及び bは、上記式（6— 1)に示すそれぞれ行列及びベクトルである、また、 add(A, b, 1)は、上記式（8)により、 Aと bに直線 lineのモーメントをカ卩える関数である。 Solve(A, b)は、 Am=bを満たす mを求め、上記式 (4)一（7)により平面パラメータ n, dを計算する。 select(open)は、例えば最初の 1つなど、 openの中力任意に 1つのエレメントを選択する関数である。また、 index(l )

1は、画素列又は行における 1のインデックス

1 を返す関数である。また、 neighbor(index)は、与えられたインデックスに隣接したインデックス、例えば {index- 1, index+1}を返す関数である。

また、上述したように、本実施の形態においては、図 19のステップ S3において領域拡張処理を行って平面方程式を更新した後、ステップ S4にお、て平面方程式を再度算出する処理 (Post processing)を行う。この再度算出する処理では、例えば上述のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離データ点又は線分の平面力のずれを計算し、所定の値以上平面から外れる距離データ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低減することができる。

次に、このステップ S4について詳細に説明する。ここでは、 2つのステップにより、平面方程式を再度算出する方法について説明する。先ず、ステップ S3にて検出された各平面の境界の距離データ点 (pixels)において、現在属している平面よりも、隣接する平面までの距離が近いデータ点が検出された場合は、当該データ点を隣接する平面の方に含める処理をする。また、いずれの平面にも属していなぐ且つ距離が例えば 1. 5cmなど比較的大きい閾値以下である平面が存在するデータ点が検出できた場合は、当該データ点をその平面に含める処理をする。これらの処理は各平面領域の境界近傍のデータ点を検索することで実行することができる。以上の処理が終了したら、再度平面方程式を算出する。

次に、上述のようにして再度算出された平面の各領域の境界近傍において、各データ点と平面との距離が例えば 0. 75cmなど比較的小さい閾値を超える場合は、それらのデータ点を捨てる処理を実行する。これにより、その平面領域は若干小さくなるものの更に精確な平面を求めることができる。距離データ点を削除後、再び平面を求め、この処理を繰り返す。このことにより、極めて精密に平面を求めることができる。次に各処理によって得られる結果を示す。図 30Aは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図 30Bは、縦軸を x、横軸を y、各データ点の濃淡で z軸を表現して 3次元距離データを示す図であり、更に、行方向の画素列力線分抽出処理にて同一平面に存在するとされるデータ点群力も直線を検出したものを示す。図 30Bに示す直線群から領域拡張処理によりえられた平面領域を図 30 Cに示す。このように、ロボット装置の視野内には、 1つの平面（床面）のみが存在する、すなわち、床面が全て同じ平面として検出されていることがわかる。

次に、床面に段差を一段置いたときの結果を図 31に示す。図 31Aに示すように、床面 Fには、 1段の段差 ST3が載置されている。図 31Bは、実験条件を示す図であり、着目点と直線 (線分)との距離力 ¾ax_dを超える場合は、データ点群を分割する。また、抽出の成否（水平）（correct extraction(horizontal))は、行方向のデータ列毎に、合計 10回の線分抽出を行う線分拡法による平面検出を行って成功した回数を示し、抽出の成否（垂直）（correct extraction(vertical))は、列方向のデータ列毎についての抽出の成否を示す。また、 No. 1— No. 5は、上述した Zig-Zag-Shape判別処理を取り入れていない従来の線分拡張法による平面検出処理の条件、 No. 6は、 Zig-Zag-Shape判別処理を行った本実施の形態における平面検出方法の条件を示す。

図 31C及び図 31Dは、線分拡張法により平面検出した結果を示す図であって、それぞれ本実施の形態における手法により平面検出した結果、従来の線分拡張法により平面検出した結果 (比較例）を示す。図 31Bに示すように、従来の手法においては、線分抽出（Line Fitting)において推定のための閾値パラメータ max_dを大きくする（ max_d= 25, 30)と検出率が下がり、閾値 max_d小さくする (max_d= 10, 15)と検出率が向上する。これに対して、本発明のように、ジグザグ形検証処理を導入することにより、大きな閾値 max_d= 30を設定しても、優れた検出結果を示すことがわかる。

すなわち、閾値 max_dを大きくすると、ノイズの影響が少なくなるものの、線分抽出が難しくなり、閾値 max_dを小さくすると、ノイズの影響を受けて誤検出が多くなつてしまう o図 32Aに示す床面を撮影した画像から 3次元距離データを取得した場合を図 32B 及び図 32Cに示す。いずれ左図は、行方向の画素列 (距離データ列)から線分を抽出した例、右図は列方向の画素列 (距離データ列)から線分を抽出した例を示す。図 32Bに示すように、閾値 max_dを小さくすると、ノイズの影響が大きくなり、ノイズの影響が大きい遠方などにおいては特に、線分をうまく検出することができない。一方、図 32Cに示すように、従来の線分抽出に更にジグザグ形判別処理を加えた場合、閾値 max_dを大きくしても、更にノイズの影響が大きい遠方の領域であっても線分が検出されていることがわ力る。

図 33及び図 34は、それぞれ異なる階段を撮影した画像カゝら 3次元距離データを取得して平面検出した例を示す図である。図 33及び図 34に示すように、いずれの場合も全ての踏面を平面として検出できている。また、図 34Bは、床面の一部も他の平面として検出成功して、ることを示す。

本実施の形態によれば、線分拡張法による平面検出を行う際、始めは大きな閾値を設定して線分を分割し、次に Zig-Zag-Shape判別処理により、閾値を超えるデータ点を持たない直線であってもジグザグ形である場合には、ノイズではなぐ複数平面からなる直線であるとして線分を分割するようにしたので、ノイズを含む距離情報から複数の平面を精度よく検出することが可能となる。

このように、小さい段差も精度よく検出することができるため、例えばロボット装置が移動可能な環境内の階段などを認識することができ、二足歩行ロボット装置であれば、この検出結果を利用して階段昇降動作が可能となる。

更に、複数の平面によって構成されている凸凹の床面を歩行可能な平面だと誤認識することがなくなり、ロボット装置の移動などが更に簡単になる。

なお、本発明は上述した実施の形態のみに限定されるものではなぐ本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。また、上述した線分抽出処理、ジグザグ形検証処理、領域拡大処理などの各処理のうち 1以上の任意の処理は、ハードウェアで構成しても、演算器 (CPU)にコンピュータプロダラムを実行させることで実現してもよい。コンピュータプログラムとする場合には、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。

Claims

請求の範囲

[1] 1. 3次元の距離データから平面を検出する平面検出装置において、

3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、

上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする平面検出装置。

[2] 2.上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にある力否力を再度推定することを特徴とする請求の範囲第 1項記載の平面検出装置。

[3] 3.上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがある力否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割する

ことを特徴とする請求の範囲第 1項記載の平面検出装置。

[4] 4.上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第 1の線分を抽出し、該距離データ点群のうち該第 1の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群力第 2の線分を抽出し、該第 2の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割する

ことを特徴とする請求の範囲第 1項記載の平面検出装置。

[5] 5.上記線分抽出手段は、上記第 1の線分を求めた距離データ点群の標準偏差が所定の閾値以上である場合には、当該距離データ点群を分割する

ことを特徴とする請求の範囲第 4項記載の平面検出装置。 [6] 6.上記平面領域拡張手段は、同一の平面に属すると推定される 1以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する

ことを特徴とする請求の範囲第 1項記載の平面検出装置。

[7] 7.上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有する

ことを特徴とする請求の範囲第 6項記載の平面検出装置。

[8] 8.上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属する力否かを推定する

ことを特徴とする請求の範囲第 6項記載の平面検出装置。

[9] 9.上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から最小二乗法により上記第 2の線分を生成する

ことを特徴とする請求の範囲第 4項記載の平面検出装置。

[10] 10.上記線分抽出手段は、 2つの撮像手段による視差によって距離を計測する距離計測手段により計測された 3次元の距離データを使用して線分抽出する

ことを特徴とする請求の範囲第 1項記載の平面検出装置。

[11] 11.上記線分抽出手段は、レーザ距離計測計により計測された 3次元の距離データを使用して線分抽出する

ことを特徴とする請求の範囲第 1項記載の平面検出装置。

[12] 12. 3次元の距離データ力平面を検出する平面検出方法において、

3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、

上記線分抽出工程にて抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、上記線分抽出工程では、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする平面検出方法。

[13] 13.上記線分抽出工程では、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にある力否かを再度推定することを特徴とする請求の範囲第 12項記載の平面検出方法。

[14] 14.上記線分抽出工程では、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、偏りがある場合には該着目点にて該距離データ点群を分割する

ことを特徴とする請求の範囲第 13項記載の平面検出方法。

[15] 15.上記線分抽出工程では、上記同一平面上にあると推定される距離データ点群から第 1の線分を抽出し、該距離データ点群のうち該第 1の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離デ一タ点群力第 2の線分を抽出し、該第 2の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割する

ことを特徴とする請求の範囲第 13項記載の平面検出方法。

[16] 16.上記平面領域拡張工程では、同一の平面に属すると推定される 1以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新するとともに該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する

ことを特徴とする請求の範囲第 12項記載の平面検出方法。

[17] 17.上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除!ヽた距離データ点群から再度平面を算出する平面再算出工程を更に有する

ことを特徴とする請求の範囲第 16項記載の平面検出方法。 [18] 18.上記平面領域拡張工程では、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属する力否かを推定する

ことを特徴とする請求の範囲第 16項記載の平面検出方法。

[19] 19. 自律的に行動するロボット装置において、

3次元の距離データを取得する距離計測手段と、

3次元の距離データから平面を検出する平面検出装置と、

上記平面検出装置による平面検出結果に基づき行動を制御する行動制御手段とを有し、

上記平面検出装置は、

上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とするロボット装置。

[20] 20.対象物に対して模様を付与する模様付与手段を有する

ことを特徴とする請求の範囲第 19項記載のロボット装置。

[21] 21.上記模様付与手段は、上記 3次元の距離データを取得する際に上記対象物に模様を投射する投射手段である

ことを特徴とする請求の範囲第 20項記載のロボット装置。

mmm w ^

PI1p FA Fllro

880脚 Z O

6£8蘭 SOO df/ェ) d

C8l700/S00Zdf/X3d 880/S00Z OAV νΐ,ιζ

C8l700/S00Zdf/X3d 4/34

PCT/JP2005/004839

FIG.4

09OU

asoid

V9OIJ

國 C8l700/S00Zdf/X3d 880/S00Z OAV

£/9

FIG.6A FIG.6B

OId

圖 C8l700/S00Zdf/X3d 880/S00Z OAV 8ΌΙ=Ι

画 C8l700/S00Zdf/X3d 880/S00Z OAV 9/34

FIG.9

FIG.10

π面 C8l700/S00Zdf/X3d 880/S00Z OAV

£/VV ujoig/uuoQi x iooexuiotr

f8tO0/S00ldT/13d

ZIZV 13/34

244 PCT/JP2005/004839

3cm X 33cm 12cm/32cm 〔〕

F.IG1 C

I 1

[s劇 £/ V

P FI1

[91园] C8l700/S00Zdf/X3d 880/S00Z OAV 17/34

WO 2005/088244 PCT/JP2005/004839

[図 17]

FIG.18

19/34

WO 2005/088244 PCT/JP2005/004839 圆 19]

FIG.19

20/34

088244 PCT/JP2005/004839

22/34

WO 2005/088244 PCT/JP2005/004839

[図 22] ジグザグ形判別

開始データ点群（Ρ[0, ··· ,η-1]) hS20

と直镍（ a，d,a )入力

FIG.22 23/34

PCT/JP2005/004839

WO 2005/088244

[図 23]

Input; pis： vector of points, n： number of points, 0t，d， σ： parameters and std-dev of fitted line. Output： true if curve contains a zig-zag- shape, false otherwise.

Sequence：

ifo > thrcshu then

valO = pts [0] *x* cos a ÷ pts [0] .y* sin Of +

count = 1

for j = l to (n-i) do

val = pts[i] .x* cos pts[i] .y* sin Ci + d

if (val* valO <=0) then

vaJO = val

count = 1

else

count = count + 1

if {count >= rnin-powts-for-zig-zag-shape ) then

return true

endif

endfor

endif

return false

FIG.23

p [ 0， ... n-1 ]

FIG.24

[S2园]

6£81-00/S00ldf/13<I WiSSO/SOOi O

ャ ε/ss 26/34

WO 2005/088244 PCT/JP2005/004839

[図 26]

FIG.26

28/34

WO 2005/088244 PCT/JP2005/004839

[図 28]

Algorithm FindSeedRegion

Input: linesfij: vector of lines for esch image row (or column) i ,

n： number of image rows (or columns)

Output： set of lines (seed region) or empty set (no seed found),

Sequence：

for 1 ^ 0 to (n-3) do

for 1, in lines[i] do

for 1₂ in lines[i+l] άο

for lj in linesfi+2] do

if overlap( I； ,l₂) and overlap( l₂ , ) then

(n,d) = fitPlane(l_t ,l₂ ,l₃)

if nns(l_s ,1₂，1₃)く thresh l_ms then

seed = {1い 1₂ ,1₃ }

removal, ,l₂ , )

return seed

endば

endif

endfor

return ノ

FIG.28 29/34

WO 2005/088244 PCT/JP2005/004839

[図 29]

Algorithm RegionGro wing

Input: region： set of lines as seed region,

lines卩]: vector of lines for esch image row (or column) /.

n： number of image rows (or columns)

Sequence：

A = 0，b = 0

for 1 m region do (A,b) = add(A,b,l) endfor

(n,d) = solve(A,b)

open = region

while not empty(open) do

1 i = select( open ), open = open - {1 ! }

for / in neighbor(index( 1；)) do

forゾ 2 in Jincsfi] do

if overlap(l_s ,1₂) ^and /ms ぐ /Ares L?^ then

region = region + {1₂ },

(A,b) ^add(A,b, l₂)> M =soive(A,b)

open = open + {1₂ },

re ove(l₂)

endif

endfor

plane = {n,d,A_fb,region}

planes =planes + {plane}

FIG.29 30/34

WO 2005/088244 PCT/JP2005/004839

[図 30]

S VOIJ ϋοε ΘΕ

FIG.31C FIG.31D

FIG.32C

33/34

WO 2005/088244 PCT/JP2005/004839

[図 33]

〜

」,ゝΓ f! ίi

overold

ST

FIG.34B