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

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, O. oldgor, K. Bowyer, D. Eggert, A. Pitzgibbon, and R. Fisher.) , 「距離画像セグメンテーションアルゴリズムの実験的比較（An experimental comparison of range image segmentation algorithms)」 , Transaction on Pattern Analysis Machine Intelligence, 18(7), 1996では、距離情報から参照点をランダムに サンプルし、その点から空間的に近傍の点を連結することによって平面を検出する手 法を提 し飞いる。まに、 Iocchiら (し Iocchi, . onolige, and M. Bajracharya.) , 「 ステレオ画像を使用した環境平面における現実の視覚地図（Visually realistic mapping of planar environment with stereo)」Int. Symposium on Bxpenmental Robotics (ISER), 2000では、距離情報からハフ変換によって統計的に平面を検出す る手法を提案している。更に、 Okadaら（K. Okada, S. agami, M. Inaba and H. Inoue.「平面セグメントの検索アルゴリズム，実装，アプリケーション（Plane Segment Pinder: Algorithm, Implementation, and Applicationsノ」 Int. Conf. on Robotics and Automation (ICRA), 2001では、距離情報からランダムに選んだ 3点を平面方程式に 当てはめ、統計的に平面を検出するランダマイズドハフ変換による手法を提案してい る。更にまた、 Jiangら（X.-Y. Jiang and H. Bunke.)「線分拡張法によって距離画像を 平面領 ¾^に尚速に分害¹ Jする手法 (Fast segmentation of range images into planar regions by scan line grouping)」 Macmne Vision and Applications, 7(2), PI 15 122, 1994では、距離画像中の列に並んだ点群はそれらの点が 3次元空間で同一平面上 にあるならば、空間内で直線となることを利用し、線分拡張法 (scan line grouping)によ る手法を提案している。

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

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

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

under-segmentationの問題が起こりやすい。また、逆に、ノイズの影響が大きい計測 データの場合に閾値を下げると、実際には一つの平面が複数の平面に分離されてし まつ over-segmentationか起こる。

上述の 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とを結ぶ線分 431aと、分割点 430cと右端のデータ点 4 30bとを結ぶ線分 431bとに分割する。これを全ての点と線分との距離が閾値以下に なるまで繰り返すことによって、与えられたデータにフィットした複数の線分を検出す ること力 Sできる。ここでは、図 5Cに示すように、最終的に 2箇所の分割点となるデータ ^,430c, 430dカ選択され、 泉分 431 (ま、 泉分 431 a、 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段目の踏面力 S、破線で囲 まれる平面 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、ずれの方法に おいても、距離計測データから正確に平面を検出することが困難であるという問題点 力 ^sある。

発明の開示

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

本発明は、このような従来の実情に鑑みて提案されたものであり、計測ノイズを含む 距離データからノイズに対してロバストにかつ複数平面を同時に正確に検出すること ができる平面検出装置、平面検出方法、及び平面検出装置を搭載したロボット装置 を提供することを目的とする。 上述した目的を達成するために、本発明に係る平面検出装置は、 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は、データの 分布が線分に対してジグザグ形である場合、図 21 Bは、ノイズなどにより線分近傍に 一様に分布して!/、る場合を示す模式図である。

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

【図 3 1】 図 3 1は、 床面に段差を一段置いたときの本実施の形態における平面検出方法 と従来の平面検出方法との結果の違いを説明するための図であって、 図 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次元距離データを使用して、 それぞれ上面、 正面、 側面から平面 を検出した例を示す図である。

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

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

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

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

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

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

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

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

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

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

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

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

このようなロボット装置は、ロボット装置全体の動作を制御する制御システムを例え ば体幹部ユニット 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等の各種の装置力 S、バス.インタフェース（I/F) 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の出力信 号により各脚部ユニット 205R/Lが遊脚又は立脚のいずれの状態であるかを検出 することによって、ロボット装置 201の全身協調運動を適応的に制御することができる 。更に、 CPU311は、 ZMP位置が常に ZMP安定領域の中心に向力、うように、ロボッ ト装置 201の姿勢や動作を制御する。

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

このようなロボット装置においては、頭部ユニット 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の視野範囲は、ロボット装置 2 01の前方の所定範囲となる。

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

本実施の形態のロボット装置 201におけるソフトウェアは、オブジェクト単位で構成 され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボ ット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を 行うこと力 Sできる。なお、ロボット装置の位置を示す座標として、例えば、ランドマーク 等の特定の物体等に基づく所定位置を座標の原点としたワールド基準系のカメラ座 標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心 (座標の原点）としたロボ ット中心座標系（以下、相対座標ともいう。）との 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台のカメラによる視差に より得ること力 Sできるため、模様がないものは視差が算出できず、正確に距離を計測 することができない。すなわち、ステレオビジョンシステムにおける距離データの計測 精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある 点が左目及び右目に写像される点の違レ、を示し、そのカメラからの距離に応じて変 化するものである。

そこで、図 17に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステ ムを構成するステレオカメラ 11R/Lを備えるとともに、例えば同じく頭部ユニットなど に投射手段としての例えば赤外光などを出力する光源 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又 feunder—segmentationなどもの 題;^生し Iしまつ。そこで、本 実施の形態においては、この線分抽出において、距離データの分布を解析すること で、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するもの とする。

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

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

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

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

一方、最大距離 distがデータ点群分割の閾値 max_dより小さ!/、場合には（S14： NO )、データ点群 Ρ [0· · ·η_1]から後述する最小二乗法によって最適な線分の方程式 lineを求め（ステップ S 15)、この方程式 lineが示す線分 L2を第 2の線分として生成す る。そして、データ点群 Ρ [0· · ·η-1]がこの線分 L2に対して後述する Zig-Zag-Shape でぁるかどぅかを調べる（ステップS16)、Zig-Zag-Shapeでなぃ場合（S16 : NO)、得ら れた線分の方程式 lineを線分抽出結果リストに追加し (ステップ S I 7)、処理を終了す また、ステップ S 16においてステップ S 15で求めた線分が Zig-Zag-Shapeである判 断された場合（S16 : YES)の場合、上述のステップ S 14と同様、ステップ S18に進み 、ステップ S13において距離 distを求めた着目点 brkにてデータ点群を 2つのデータ 点群 Ρ [0· · 'brk]及び P[brk' · ·η-1]に分割する。このステップ S18にて 2つのデータ 点群が得られた場合には、それぞれを再帰的に再度ステップ S 11からの処理を行う 。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すな わち全てのデータ点群がステップ S 17を経るまで処理を繰り返し、これにより、全ての 線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群 Ρ

[0· · ·η-1]からノイズの影響を排除し複数の線分からなる線分群を精度よく検出する こと力 Sでさる。

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

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

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

[数 3]

次に、ステップ SI 6におけるジグザグ形（Zig-Zag-Shape)判別方法につ!/、て説明す る。この Zig-Zag-Shape判別では、ある n個のデータ点群 Ρ[0· · ·η_1]と直線 Line , d)、 xcosa +ycosa +d = 0力 S与えられたとき、そのデータ点群 Ρ[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, σ )とを入力する（ステップ S20)。 ここで、 σは、点列の標準偏差を示す。次に、この標準偏差 σが所定の閾値 th_(jよ り大きいか否かを判断する。この標準偏差 σが閾値 th_(jより小さい場合 (ステップ S2 l:No)は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、 判別を終了する。そして、標準偏差 σが閾値 th_(rより大きい場合のみ判別処理を継 続する。次に、データ点群 Ρ [0 · · · η-1]のうちの最初のデータ点 P [0]が直線のどちら 側にある力、を sing(sdist(P[0]》によって判断し、この結果を valに代入するとともに valと

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

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

0

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

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

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

0

ト値 countに 1を代入し（ステップ S28)、データ点カウンタのカウント値 iをインクリメント 一方、ステップ S26において、 valと valとが同じ場合（ステップ S26 : YES)、データ

0

点 P [i— 1 ]と P [i]は、直線 Lineに対して同じ側にあると判断され、連続点カウンタの カウント値 countを 1つインクリメントする（ステップ S 27)。さらに、連続点カウンタのカウ ント値 countが Zig-Zag-Shapeと判定されるための最小のデータ点数 min_cより大きい か否か判定し（ステップ S29)、大き!/、場合には（ステップ S29： YES)、

Zig-Zag-Shapeと判断し、 TRUEを出力して処理を終了する。一方、連続点カウンタの カウント値 countが最小のデータ点数 min_cより小さ!/、場合には（ステップ S29： NO)、 ステップ S 30に進み、データ点カウンタのカウント値 iをインクリメントして（ステップ S 30 )、ステップ S24からの処理を繰り返す。

そして、このステップ S24からの処理を、データ点カウンタのカウント値 iがデータ点 数 nに到達するまで続け、カウント値 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〜ス テツプ S 30までの処理は図 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, z)により表され、平面の方程式はその法線べタト ル n(nx, ny, nz)と非負の定数 dによって下記式（4)で表される。

[数 4コ xⁿx ⁺ yn_y+zn_z +d = 0 ".(4) ここで、図 11に示すように、ステレオカメラでは、焦点を通る平面を観測することが できない、すなわち、平面は焦点を通らないため、 d≠0とすることができる。したがつ て、平面は、最小二乗法により下記式（5)に示す値を最小にする値として求めること ができる。

[数 5コ (n，め =∑(pfn +め²' —(5) 最適解は n = m/ II m II , d=— 1/ || m ||として求まる。ここで、 || · ||は、ベクトル の大きさ、 mは、行列式によって連立一次方程式を解くクラメールの法則（Cramer's rule)を使用して下記（6-1)のように容易に得られる線形システムの解である。

[数 6]

A m = b -- (6-1)

ここで、

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

i i この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であ つても、上記式 ½— 2)に示す Aと bの値を更新するのみで、平面パラメータを再計算 すること力 Sできる。更に、本実施の形態における線分抽出方法の場合は 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)を 求めること力 Sでさる。

[数 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)を 使用して上記 ½ 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 等によって獲得された定数を用いることも可能だ力 s、閾値の不適切な設定による

Under-Segmentation, Over-Segmentationの問題が生じる為、線分に含まれる距離デ ータの性質によって適応的に決定されることが望ましい。ノイズを多く含む線分群に 対して低い閾値を設定すると、多くの線分が異なる領域に分割され、領域拡張が適 切に fiわれない。

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

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

th 2 = max (th 3, d sigma(l )) …（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のようにも表現することができる。 overlapd , 1 )は、各イメージロウに含まれる直線ベクトル 1と 1における端点間の位置が直線べ j k j k

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

1 2 3

計算し、上記式（8)により算出された A, により、直線ベクトル 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)を行う。この再度算出する処理では、例えば上述 のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離デ ータ点又は線分の平面からのずれを計算し、所定の値以上平面から外れる距離デ ータ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低 減すること力 Sでさる。

次に、このステップ 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. l ~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を小さくすると、ノイズの影響を受けて誤検出が多くなつてしまう 。図 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次元の距離データから平面を検出する平面検出装置と、

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

上記平面検出装置は、

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

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

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

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

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

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