JP4535096B2 - 平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置 - Google Patents

Description

本発明は、視差又は距離画像等の３次元データから平面を抽出する平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置に関し、特に、平面抽出における信頼度及び演算速度を高めた平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置に関する。

電気的又は磁気的な作用を用いて人間（生物）の動作に似た運動を行う機械装置を「ロボット」という。我が国においてロボットが普及し始めたのは、１９６０年代末からであるが、その多くは、工場における生産作業の自動化・無人化等を目的としたマニピュレータ及び搬送ロボット等の産業用ロボット（Industrial Robot）であった。

最近では、人間のパートナーとして生活を支援する、即ち住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットの開発が進められている。このような実用ロボットは、産業用ロボットとは異なり、人間の生活環境の様々な局面において、個々に個性の相違した人間、又は様々な環境への適応方法を自ら学習する能力を備えている。例えば、犬又は猫のように４足歩行の動物の身体メカニズム及びその動作を模した「ペット型」ロボット、或いは、２足直立歩行を行う人間等の身体メカニズム及びその動作をモデルにしてデザインされた「人間型」又は「人間形」ロボット（Humanoid Robot）等のロボット装置は、既に実用化されつつある。

これらのロボット装置は、産業用ロボットと比較して、例えばエンターテインメント性を重視した様々な動作等を行うことができるため、エンターテインメントロボットと呼称される場合もある。また、そのようなロボット装置には、外部からの情報及び内部の状態に応じて自律的に動作するものがある。

ところで、自律型ロボット装置において、自身の周りの環境を認識して経路計画を立て、それに応じて移動する能力は言うまでもなく重要である。環境を認識するためには、ロボット装置の周囲の障害物情報等が必要である。従って、ロボット装置は、距離画像又は視差画像等の３次元データから床となる平面を検出し、障害物を認識する必要がある。

３次元データから平面を抽出する方法としては、ハフ変換（Hough Transform）を使用する方法がある。ハフ変換には、Forward Position，Backward Position，Feature Point Pairs等といわれるいくつかの方法がある。以下、これらのハフ変換について、簡単のため、２次元画像平面内の直線検出の例として説明する。元画像からは何らかの画像処理により、エッジ（直線上にのると予想される特徴点）が検出されているものとする。また、特徴点の数をＭとし、元画像の大きさをＬ×Ｌ、投票先のパラメータ空間（投票箱）の大きさをｌ（エル）×ｌ（エル）とする。

Forward Positionでは、元画像の特徴点を順次たどり、各点をパラメータ空間内に写像したときの曲線が通るスロット１つ１つに投票していく。アルゴリズムの複雑度はＯ（Ｍｌ）である。

Backward Positionでは、パラメータ空間内の各スロットを順次操作し、各スロットを元画像に対し逆写像した直線上にのっている特徴点の数をそのスロットの得票とする。アルゴリズムの複雑度はＯ（Ｌｌ_２）である。

Feature Point Pairsでは、特徴点２点のあらゆる組み合わせを順次たどり、その２点の組から計算されるパラメータが属するスロッドに得票する。アルゴリズムの複雑度はＯ（Ｍ_２）である。

しかしながら、これらのハフ変換により３次元データから平面を抽出する方法は、演算量及び使用メモリが極めて多くなる。これは、次元数が２次元よりも１つ多い上に、全てのデータポイントが特徴点としての意味を有するためである。従って、これらのハフ変換は、３次元データからの平面検出というタスクにおいては、演算量及び使用メモリの観点から何れも現実的ではない。

また、ランダム化ハフ変換（Randomized Hough Transform：ＲＨＴ）と呼ばれるハフ変換がある。このランダム化ハフ変換は、上述したFeature Point Pairsの派生であり、上述の２次元画像平面内においては、特徴点２点のあらゆる組み合わせではなく、ランダムサンプリングによるＫ通りの組み合わせについてのみ処理を行うものである。このアルゴリズムの複雑度はＯ（Ｋ）である。

従来、３次元データから平面を検出するためにハフ変換を利用した方法が提案されている。例えば、特許文献１には、３次元データに対してハフ変換処理をして平面物体の３次元空間における位置を推定する物体検出方法及び平面推定方法が開示されている。特許文献１に記載の技術においては、３次元データである視差画像からハフ曲線をある平面上にプロットし、これらのハフ曲線が最も多く通過する平面上の点を求め、これに基づき、ある平面上にあると仮定した平面直線群の垂線の傾き及びＹ軸までの距離を補正して平面物体を推定している。

また、特許文献２には、平面上の距離データを精度よく測定することを目的とした平面推定方法が開示されている。特許文献２に記載の技術においては、ステレオ画像を矩形小領域に分割してこの小領域毎に距離データを求め、この小領域毎を水平方向にグループ化した大領域毎に、これらの領域に含まれるＭ個の距離データからハフ変換により平面通過直線を当てはめ、これを全ての大領域について行うことにより撮像空間における平面を推定するものである。

更に、特許文献３には、メモリ容量及び処理時間短縮を図った画像処理装置が開示されている。この特許文献３に記載の技術においては、３次元空間から直線群を入力し、この直線群の法線ベクトルを特徴空間に投票し、これに基づき所定の本数の直線を含む平面を抽出するものである。

また、特許文献４には、画像のフレームの時間的な系列を表す時系列画像の画像特徴を抽出する技術として、３次元ハフ変換としてパラメータ空間に対して投票を行い、平面を抽出する方法及びその装置が記載されている。

特開平１０−９６６０７号公報 特開平９−８１７５５号公報 特開平７−２７１９７８号公報 特開２０００−０１１１８２号公報

しかしながら、特許文献１乃至特許文献３に記載の技術においては、投票空間に対して平面パラメータを求め直接投票するものではないため、処理が煩雑になると共に、ハフ変換が有する確率推定としての利点を十分に生かすことができないという問題点がある。

また、ハフ変換を利用すると、処理時間が長くなり、高速化が困難であるという問題点もある。

このように、ノイズが多い３次元データから未知の平面パラメータを決定する、即ち、平面を検出する方法は確立されていない。

本発明は、このような従来の実情に鑑みて提案されたものであり、ノイズに対してロバストであり、信頼性が高い平面を抽出することができる平面抽出方法、その装置、そのプログラム、その記録媒体及び撮像装置を提供することを目的とする。

上述した目的を達成するために、本発明に係る平面抽出方法は、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、制御手段が、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、制御手段が、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、制御手段が、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなることを特徴とする。

本発明においては、３次元データから平面パラメータ算出し、この平面パラメータを投票空間に直接投票するため、ノイズの多い３次元データからでも平面を安定して抽出することができる。

上記平面算出工程は、上記平面の法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたとき、上記平面パラメータ（θ，φ，ｄ）を算出する工程であって、上記投票空間は、下記数式（１−１）又は数式（１−２）で表されるものとすることができる。

これにより、平面を指定する平面パラメータは、元の３次元直交座標空間に対して極座標を形成することから、投票スロット（グリッド）が元の対応空間内で同一の面積を占めるように投票空間を上記数式（１−１）又は数式（１−２）として設計することにより、更にロバストで精度が高いデータが得られる。

また、上記３次元データ群は、入力画像を対応する３次元データ群に変換されるものであって、各上記３次元パラメータは上記入力画像に基づき算出される信頼度パラメータを個別に有し、この信頼度パラメータに基づいて上記３次元データ群の３次元データを選別するデータ選別工程を有することができ、信頼度が低い３次元データを破棄すること等ができ、処理結果の信頼性及び安定性を向上することができる。

更に、上記ランダムサンプリングを複数回繰り返すことができる。更にまた、３点のデータから直接解法によって上記平面パラメータを算出する工程を有することができ、サンプリングデータから上記投票空間へ投票するハフ変換により高精度の平面パラメータを推定することができる。

また、上記サンプリングデータは、３次元空間を所定の領域に分割しこの領域内の３次元データとすることができる。更にまた、３点以上の上記３次元データから共分散行列を固有値展開して最小固有値に対応するベクトルを抽出する主成分分析により上記平面パラメータを算出するか、又は上記平面が原点を通らない任意の平面であると仮定し最小二乗法を使用して上記平面パラメータを算出することができる。

更に、上記平面決定工程は、上記投票を繰り返し、上記投票の総数が所定の閾値を超えた場合、又は上記投票数に基づいて決定される投票値が最も高い位置における投票値の合計が全ての投票値の合計である総投票値の所定の割合に達した場合に投票をうち切る工程を有することができるため、処理を高速化することができる。

更にまた、上記平面決定工程は、上記３次元データの信頼度及び／又は平面算出方法により異なる重みを定めて投票する重付投票工程を有するため、データの信頼度が高く精度が向上するため、投票総数を減らすことができ、高速処理が可能となる。

また、上記平面決定工程は、上記投票空間の投票のピーク近傍における投票数による重み付き平均を行う重付平均工程を有することができ、投票空間の量子化サイズよりも細かい精度でパラメータを推定することができるため、量子化サイズを大きくして処理の高速化を図ることができる。

更に、上記平面決定工程により抽出された平面に含まれる３次元データが２次元となる座標変換行列を求め、上記３次元データを該座標変換行列により写像して出力する写像工程を有することができ、これにより、後段の処理が容易になる。

更にまた、上記平面決定工程は、上記投票結果により推定された平面パラメータを初期パラメータとしてイタレーションにより該初期パラメータから最適パラメータを算出する最適パラメータ算出工程を有することができるため、投票による平面パラメータ推定段階においては、ある程度の精度で推定しておけば、最適パラメータ計算により精度を向上することができるため、更に高速化が可能となる。

また、上記平面決定工程は、上記投票における投票ピークの鋭さ、上記イタレーションに使用したデータポイント数、及び上記イタレーションにより算出される誤差残差からなる群から選択された１以上を信頼度のパラメータとして使用し、決定される平面の信頼度を算出する信頼度算出工程を有することができるため、抽出された平面の精度が明確になり、精度が低いデータについては破棄する等してデータを選別することができる。

本発明に係る平面抽出装置は、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する画像処理手段と、上記３次元データ群から、上記画像処理手段において算出された上記信頼度パラメータに基づいて、上記３次元データを選別するデータ選別手段と、上記データ選別手段にて選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出手段と、この平面算出手段から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定手段とを有し、上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなることを特徴とする。

本発明に係るプログラムは、所定の動作をコンピュータに実行させるためのプログラムであって、コンピュータが、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、コンピュータが、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、コンピュータが、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、コンピュータが、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなることを特徴とする。

本発明に係る記録媒体は、所定の動作をコンピュータに実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体であって、コンピュータが、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、コンピュータが、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、コンピュータが、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、コンピュータが、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなることを特徴とする。

本発明によれば、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出し、３次元データ群から、算出された信頼度パラメータに基づいて３次元データを選別し、選別された３次元データから３点以上の３次元データをサンプリングし、３次元データによって決まる１つの平面を示す平面パラメータを複数算出し、得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定し、サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなるため、平面パラメータを投票空間に直接投票することができるため、ノイズに対してロバストで、精度が高く、従って、サンプリング数を減らして処理を高速化することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。本実施の形態においては、平面抽出装置が搭載された２足歩行のロボット装置について説明する。

図１は、本発明の実施の形態におけるロボット装置の概略を示すブロック図である。図１に示すように、ロボット装置１の頭部ユニット２５０には、２台のＣＣＤカメラ２００Ｒ，２００Ｌが設けられ、このＣＣＤカメラ２００Ｒ，２００Ｌの後段には、ステレオ画像処理装置２１０が設けられている。２台のＣＣＤカメラ（以下、右目２００Ｒ、左目２００Ｌという。）により撮像された右目画像２０１Ｒ、左目画像２０１Ｌは、ステレオ画像処理装置２１０に入力される。ステレオ画像処理装置２１０は、各画像２０１Ｒ，２０１Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像（ＹＵＶ：輝度Ｙ、ＵＶ色差）２０２及び視差画像（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）２０３をフレーム毎に左右交互に算出する。ここで、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。

このカラー画像２０２及び視差画像２０３はロボット装置１の体幹部２６０に内蔵されたＣＰＵ（制御部）２２０に入力される。また、ロボット装置１の各関節にはアクチュエータ２３０が設けられており、ＣＰＵ２２０からの指令となる制御信号２２１が供給されて、その指令値に応じてモータを駆動する。各関節（アクチュエータ）には、ポテンショメータが取り付けられ、その時のモータの回転角がＣＰＵ２２０へ送られる。このアクチュエータに取り付けられたポテンショメータ、足底に取り付けられたタッチセンサ及び体幹部に取り付けられたジャイロ・センサ等の各センサ２４０は、現在の関節角度、設置情報、及び姿勢情報等の現在のロボット装置の状態を計測し、センサデータ２４１としてＣＰＵ２２０へ出力する。ＣＰＵ２２０は、ステレオ画像処理装置２１０からのカラー画像２０２及び視差画像２０３と、各アクチュエータ２３０の全ての関節角度等のセンサデータ２４１とが入力され、後述するソフトウェア構成を実現する。

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

図２は、本実施の形態におけるロボット装置のソフトウェアの構成を示す模式図である。同図において、丸で表されているのが、オブジェクト又はプロセスと呼ばれるエンティティである。オブジェクト同士が非同期に通信し合うことで、システム全体が動作する。各オブジェクトはメッセージ通信と共有メモリを使用したオブジェクト間通信方法によりデータの受け渡し及びプログラムの起動（Ｉｎｖｏｋｅ）を行っている。

図２に示すように、ソフトウェア３００は、キネマティックオドメトリ（Kinematics Odometry）ＫＩＮＥ３１０、平面抽出部（Plane Extractor）ＰＬＥＸ３２０、障害物グリッド算出部（Occupancy Grid）ＯＧ３３０、ランドマーク位置検出部（Landmark Sensor）ＣＬＳ３４０、絶対座標算出部（Localization）ＬＺ３５０及び行動決定部（Situated behavior Layer）ＳＢＬ３６０から構成され、各オブジェクト単位にて処理がなされる。これらのソフトウェアの詳細な構成及び動作については後述する。また、ロボット装置についての詳細な構成及び動作についても後述する。

ここで、先ず、上述したロボット装置１に搭載される平面抽出装置、即ち、上述した平面抽出部ＰＬＥＸ３２０の平面抽出方法について説明する。

本発明の平面抽出方法は、３次元データからランダムにデータをサンプリングして平面パラメータを算出し、ハフ変換（ランダム化ハフ変換：Randomized Hough Transform）、即ち、平面パラメータを投票空間に直接投票して平面であるか否かを決定するものである。即ち、下記数式（２）に示すＮ固の３次元データから、法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたときの平面パラメータ（θ，φ，ｄ）、即ち下記数式（３）に示す平面方程式を求める方法である。

そして、投票、即ちヒストグラムに基づく確率密度関数の推定により３次元データから、その３次元データに含まれる支配的な平面のパラメータを決定する確率論的方法である。

以下、本発明の実施の形態における平面抽出方法ついて詳細に説明する。図３は、本実施の形態における平面抽出方法の工程を示すフローチャートである。

例えば、ステレオ視（ステレオカメラ）等による方法、又はレンジセンサ等の操作によって得られた視差又は距離画像は、適切なキャリブレーションに基づく変換により、３次元データに容易に変換することができ、この３次元データの配列が入力される。

先ず、Ｎ個のデータ（３次元データ）｛ｐ_１，ｐ_２，ｐ_３，…，ｐ_Ｎ｝からＭ組のサブセット｛ｐ_１１，ｐ_２１，ｐ_３１，…，ｐ_Ｎ１｝，｛ｐ_１２，ｐ_２２，ｐ_３２，…，ｐ_Ｎ２｝，…，｛ｐ_１Ｍ，ｐ_２Ｍ，ｐ_３Ｍ，…，ｐ_ＮＭ｝を取り出す（ステップＳ１）。このときの投票総数はＭとなる。次に、各サブセット｛ｐ_１ｉ，ｐ_２ｉ，ｐ_３ｉ，…，ｐ_Ｎｉ｝から、パラメータ（θ_ｉ，φ_ｉ，ｄ_ｉ）を計算する（ステップＳ２）。次いで、計算されたパラメータを量子化し、投票先グリッドを決定し、これに対して各パラメータ毎に個別に投票を行う（ステップＳ３）。そして、最終的に投票数が多いパラメータを解とする。即ち、このパラメータを有する平面を平面として推定し、後述するイタレーション等の処理を行い、平面を決定する（ステップＳ４）。

先ず、ステップＳ１でデータを取り出す前に行われる処理について説明する。図４は、ステップＳ１の前工程を示すフローチャートである。例えば距離画像又は視差画像等のような形で得られた３次元座標データ群を処理する方法について説明する。図４に示すように、ステレオ視４０１又はレンジセンサ４０２からのデータが入力され、キャリブレーション又はパラメータ変換されて３次元のデータ配列が生成される（ステップＳ０１）。ここで、３次元座標に変換された３次元データ群と共に、この３次元データの信頼度ｒを示す信頼度パラメータがステレオ視４０１又はレンジセンサ４０２から入力される（ステップＳ０２）。本実施の形態においては、ステレオ視等に基づく距離画像を入力画像とし、ステレオ距離計算の過程で得られる信頼度パラメータを有効利用することができる。また、このような信頼度パラメータが利用不可能である場合、これを定数と見なすことで信頼度パラメータが付随しない３次元データにも直接応用することができる。

信頼度パラメータについては様々な評価値を用いて求めることができるが、本実施の形態においては、テンプレートの分散値による信頼度パラメータの求め方及びマッチングスコアによる信頼度パラメータの求め方の２つの方法について説明する。

上述したように、本実施の形態の３次元データは、ステレオ視による距離画像を入力としている。ステレオ距離計測では左右のステレオ画像中の画素の対応関係をテンプレートマッチングによって探索する。例えば、左のステレオ画像を基準画像、右のステレオ画像をマッチング先の画像とし、マッチングテンプレート内の画素の輝度の分散値を求める。そして、探索する全てのテンプレートの分散値を算出し、分散値の大きさを信頼度パラメータとすることができる。即ち、分散値が高い程信頼度が高いことを示し、分散値が低い程、信頼度が低いことを示す。

また、マッチングスコアによる信頼度画像の求め方としては、基準画像におけるテンプレートと、マッチング先画像のエピポーララインに沿って選択された領域とを比較し、これらの領域内の画素の輝度の差からマッチングスコア値を算出する。そして、マッチングスコア値、最小マッチングスコア値付近におけるピークの幅、及びグラフの急峻度値等からマッチングスコアによる信頼度パラメータを求めることができる。即ち、マッチングスコア値が低く、最小スコア値付近におけるピークの幅が小さく、グラフの急峻度値が大きいもの程、信頼度が高いことを示す。

このように、各３次元パラメータは個別に信頼度パラメータが算出され、この信頼度パラメータにより、信頼度が所定の閾値より低いデータは切り捨てる等して入力データの選別が行われる（ステップＳ０３）。上述した何れの従来例においても、入力３次元データの信頼性に関する考慮が全くなされてないため、ノイズが多く紛れ込みやすいデータとなり、このノイズにより性能が悪化するという問題点があったが、本実施の形態のように、信頼度パラメータによる３次元データの選別を行うことにより、信頼性及び安定性を向上し、性能悪化を防止することができる。

次に、ステップＳ１におけるＮ個の３次元データからＭ組のサブセットを取り出す方法について説明する。図５は、ステップＳ１乃至ステップＳ３の工程を示すフローチャートである。上述したステップＳ０３にて選別された３次元データ群（ｘ，ｙ，ｚ）及び各データに付随する信頼度パラメータ（信頼度ｒ）が出力され（ステップＳ１１）、これらのデータ群から、平面パラメータを推定するためのサンプルを抽出するサンプリングを行う。サンプリングの方法としては、３次元データ（ｘ，ｙ，ｚ）から
１．全くランダムにサンプリングする方法
２．基準点となる１点を３次元データ全体からランダムにサンプリングし、残りのデータを基準点近傍点、例えば３次元空間内での距離が十分近いデータ群等からサンプリングする方法
３．３次元データを複数の解像度レベルに亘ってスキャンしながら各小近傍（例えば、３次元空間内での距離が十分近いデータ群等）のデータをひとまとまりにしてサブセットとする方法
等がある。

３次元空間内での距離が十分近いデータ群とは、例えば基準点との距離が閾値ｄ_ｍｉｎ乃至ｄ_ｍａｘであるような任意の点とすることができる。

ここで、入力データ中には、支配的な単一の平面が存在することを仮定したとき、又は何らかの付加情報により支配的な平面が存在することを知ることができたとき等、平面領域が大きい場合には画面全体から高域にサンプリングすると効果的となる。また、小規模領域におけるデータをサンプリングする場合は、小さい段差等を多く含む画像に効果的となる。

次に、ステップＳ２における平面パラメータの算出方法について説明する。図５に示すステップＳ２１においては、３種類の平面パラメータの算出方法を有し、必要に応じて最適な算出方法をとることができる。先ず、第１のパラメータ算出方法は、３点の３次元データから直接解法を使用してパラメータを決定する方法である。

平面αは、法線ベクトルの向きと座標原点からの距離という３つのパラメータによって決定される。即ち、図６に示すように、法線ベクトルの向きを（θ，φ）で極座標表示し、原点からの距離をｄとしたとき、下記数式（４）が平面αを決定する方程式となる。

また、別の見方をすれば、上記数式（４）は、下記数式（５）に置き換えることもできる。

従って、同一直線にない３点の座標が与えられれば上記数式（４）を満たす（θ，φ，ｄ）を求めることは極めて容易であり、どちらか一方が求まれば他方も直ちに決定される。

例えば、３点｛ｐ_０，ｐ_１，ｐ_２｝が与えられているとき、下記数式（６）で示す平面の方程式は、ベクトルｍ＝（ｐ_１−ｐ_０）×（ｐ_２−ｐ_０）を使用して、下記数式（７）によって一意に計算される。

なお、一般性を損ねないものと仮定して、３点は同一直線上にはなく（｜ベクトルｍ｜≠０）、３点のうち１点ｐ_０は原点と一致しない（｜ｐ_０｜≠０）ものとする。

第２のパラメータ算出方法は、主成分分析による平面パラメータの推定方法である。即ち、画面全体を適当な分解粒度・解像度でスキャンして入力される３次元データを３次元空間内の近傍領域毎に分割する等して得られた各データ群に対し、主成分分析を行って平面パラメータを推定する方法である。

先ず、同一平面上にあると期待されるＮ個の３次元データを下記数式（８）とする。

上記数式（８）から、これらの共分散行列Ｃ＝ＰＰ_Ｔは、（理想的には）ランク（階数）２の３×３行列となる。即ち、Ｃを固有値分解し、最小固有値に対応する個有ベクトルを取れば、これが平面の法線ベクトルとなる。ここで、第１及び第２主成分は、平面を張る２つのベクトルを与え、これらに垂直なベクトルが第３主成分となる。こうして主成分分析から下記数式（９−１）に示す平面方程式は、下記数式（９−２）により計算される。

但し、ｖ_３は、最小の固有値λ_３に対応する固有ベクトルである。こうして、第２の方法によっても平面パラメータ（θ，φ，ｄ）は直ちに計算することができる。

また、平面パラメータを求める第３のパラメータ算出方法は、最小二乗法を使用して平面パラメータを当てはめる方法である。例えば、上述と同様に、画面全体を適当な分割粒度・解像度でスキャンし、得られた距離画像等を例えば矩形小領域に分割する等して、３次元空間内の近傍領域毎に３次元データ群を分割し、線形最小二乗法を使用して平面パラメータを算出する方法である。

これは、本発明の実施の形態が適用される状況に巧妙に特化した方法であり、数学的には汎用の方法ではない。即ち、推定すべき平面は座標原点を通らないと仮定して求める方法である。この仮定により、以下に説明する簡明な解法が適用可能となる。

先ず、同一平面上にあると期待されるＮ個の３次元データを上記数式（８）とし、この平面の方程式を下記数式（１０）とする。

上記数式（１０）で示される方程式は、座標原点を通らない任意の平面を表す方程式である。ここで、パラメータΓ＝（α，β，γ）には、何の制約条件も付いていない。このとき、解くべき方程式は、下記数式（１１）となる。

従って、特異値展開等の方法によりＰの一般化逆行列Ｐ_−１を計算すれば、下記数式（１２）に示すパラメータΓが、下記数式（１３）に示す誤差残差ｒを最小二乗法における意味で最小にする最適解を与える。

なお、ここで、下記数式（１４）が成り立つ。

次に、ステップＳ３における投票について説明する。平面を決定するパラメータとして（θ，φ，ｄ）を使用することは極めて自然である。ここで、平面の法線ベクトルｎは原点を中心とする半径１の単位球の表面上に存在するとみることができ、下記数式（１５）に示す関係が成り立つ。

しかしながら、法線ベクトルの微少変化を考えると、ヤコビアンＪは下記数式（１６）となるため、平面を決定するパラメータ（θ，φ，ｄ）をハフ変換を使用する際の投票先の空間の直交軸を張るパラメータとして利用するのは適切ではない。

即ち、パラメータ（φ，θ）の極座標をとると、図７（ａ）に示す投票空間５００において、各グリッド５０１は同一面積であるであるのに対し、図７（ｂ）に示すように、実際の空間に示す面積は異なる。即ち、実際の空間を球体５１０としたとき、例えば球体の中心を通り紙面に平行且つ上向きのＡの直線を中心軸とし、上記中心から中心軸に垂直な方向の球体５１０上のグリッド５１１の面積を例えばＳとすると、上記中心から中心軸方向の球体５１０上のグリッド５１２の面積はＳｃｏｓθとなり、グリッドの面積が異なる。従って、図７（ａ）に示す投票空間５００に投票すると、実際には、異なる面積のグリッド上に投票することになり、適切な投票値を得ることができない。

このように、投票先の３次元パラメータ空間を素朴に設計して投票した場合、元の対応空間内で異なる面積のグリッドに投票することになり、ピークが鋭く現れない場合があり、ノイズに対する脆弱性となる。

ここで、本願発明者等は投票空間下記数式（１−１）又は数式（１−２）で定義すれば、上述の問題を解決することができることを知見した。

図８は、上記数式（１−１）に示す投票空間を示す模式図である。図８（ａ）に示すように、投票空間６００を上記数式（１−１）で定義することにより、投票空間６００に仕切られた直交グリッド６０１のひとつひとつが、図８（ｂ）に示す実際の３次元空間６１０における平面の微小変化に対し、一様に対応付けられるとみなすことができる。上記数式（１−１）のように仮定することにより、例えば紙面に平行な方向の球体の中心を通る直線を中心軸とすれば、中心軸に平行な方向のグリッド６１１及び中心軸に垂直な方向のグリッド６１２は共に面積Ｓとなり、グリッドサイズが等しくなる。また、投票空間を上記数式（１−２）で表される空間としても、各グリッドは、元の対応空間内で同一の面積を占めるものとなる。なお、投票空間は、その各グリッドが対応空間内で同一の面積を占めるように設計すればよく、上記数式（１−１）及び（１−２）に示されるものに限らない。

このように、平面を指定する平面パラメータは、元の３次元直交座標空間に対して極座標を形成することから、投票スロット（グリッド）が元の対応空間内で同一の大きさとなるように投票空間を上記数式（１−１）又は（１−２）として設計することにより、更にロバストで精度が高いデータが得られ、従ってサンプリング数を減らすことができ、処理が高速化する。

次にステップＳ３の投票について説明する。一般に、投票の際の１票の重みは同じである場合が多い。即ち、例えば１の投票に対しその投票値を１とする。これに対して、本実施の形態においては、誤差の性質を考慮して一票の重みを定め、ノイズに対するロバスト性を向上する。即ち、１の投票に対し、誤差の評価によって、その投票値を例えば０．５又は２等とするものである。投票数誤差の評価方法としては以下に説明する方法等がある。

先ず、第１の誤差評価方法としては、投票先を計算（平面パラメータを計算）するために使用した元の３次元データの信頼度（信頼度パラメータ）を使用する方法である。この場合、平面パラメータを求めるのに使用した各３次元データに付随する各信頼度を平均する等して投票値に重み付けすることができる。

第２の誤差評価方法は、投票先を計算するために使用した元の３次元データの誤差の標準偏差を使用する方法である。この誤差とは、例えば、３点から直接解法によって平面パラメータを求める第１のパラメータ算出方法であれば誤差０、主成分分析によって平面パラメータを求める第２のパラメータ算出方法であれば本来０であるべき最小固有値λ_３から計算した値、最小二乗法によって平面パラメータを求める第３のパラメータ算出方法であれば誤差残差ｒから計算した値等とすることができる。

第３の誤差方法としては、３次元距離データがステレオ視装置、又はレンジセンサ等から得られる場合、一般的に視差方向における誤差は極めて小さく、距離方向における誤差は、距離の測定値に比例する。このことにより、計算元の３次元データの分布を視点中心の球面に投影したときの立体角が大きい程、測定誤差が計測結果に与える影響が小さくなるため、この立体角に応じて投票値に重み付けをする。また、立体角が同じ場合は実空間でのデータの分布の分散が大きい程、特に、主成分分析による第２の方法においては、第１主成分と第２主成分との積λ_１λ_２が大きい程、測定誤差が計算結果に与える影響が小さい。このことを考慮して投票値の重み付けを行うことができる。

このようにして誤差評価によって、１票の投票値に対して異なる重み付けを行うことにより、入力データのバラツキ、距離画像等を撮像する装置の特性によるバラツキ等、及びサンプルリングの仕方によるバラツキ等を低減することができる。また、これらのことから、計算結果の誤差に対するセンシティビティの指標を得ることができる。

これらの誤差の評価値によって、投票の際の１票の投票値の重みを異ならせ、ノイズに対してロバストな投票値を得ることができる（ステップＳ３１）。そして、投票総数が閾値Ｎ_ｍａｘを超えたか、又は投票総数が閾値Ｎ_ｍａｘを超え且つ投票値の分布が十分なピークを形成しているか否かを判定する（ステップＳ３２）。十分なピークとは、ピーク位置のグリッドにおけるデータが例えば全ての投票の数（総投票数）の一定以上の割合を有するか、又はピーク位置のグリッドにおける投票数の合計（得票数）又は投票値の合計（得票値）が所定の閾値以上であるか否か等により判定することができる。ここで、十分であると判定された場合は、ピーク位置又はその近傍のグリッド内の平面パラメータを後述する方法にて平均処理する。一方、十分でないと判定された場合は、ステップＳ２１に戻り、３次元データ群からデータサブセットを抽出して平面パラメータを算出する。

次に、平面を決定するステップＳ４について説明する。図９及び図１０は平面決定工程を示すフローチャートである。ステップＳ４の平面決定においては、図９に示すように、先ず、ピーク値近傍における重み付き平均処理が行われる（ステップＳ４１）。この重み付き平均処理により、ピークのより正確な位置を推定することができる。

投票空間のピーク値近傍グリッド集合をｇとすると、この集合ｇに含まれる各グリッドＧ_ｉの得票値（投票値の合計）をｖ_ｉ、代表パラメータを（θ_ｉ，φ_ｉ，ｄ_ｉ）とすれば、推定パラメータ（θ_ｋ，φ_ｋ，ｄ_ｋ）は下記数式（１７）として算出される。

上記数式（１７）に示すように、各グリッドの代表パラメータの量子化サイズよりも細かい粒度でのパラメータ推定が可能となる。ここで、上記数式（１７）では、ピーク値近傍においては、確率分布がほぼ正規分布をなすと仮定している。

図９に示すように、ステップＳ４１での重み付き平均処理により、推定パラメータの初期値（初期パラメータ）（θ_０，φ_０，ｄ_０）を出力する。次に、この初期パラメータ（θ_０，φ_０，ｄ_０）と３次元座標７００とからイタレーションを計算する（ステップＳ４２）。このイタレーションにより、更に正確な平面パラメータを推定する。そして、図１０に示すように、ステップＳ４２のイタレーションによるパラメータの変化量が収束するか、又は所定の回数以上繰り返すかして終了判定を行い（ステップＳ４３）、その結果の信頼度を判定し（ステップＳ４４）、終了判定された平面パラメータ（θ，φ，ｄ）及びその信頼度を出力する。

本実施の形態においては、３次元データはフィルタバンク（ＦＢＫ）により、リアルタイムに多重解像度化されており、上述のハフ変換による初期パラメータ推定の際には低解像度の３次元データを使用して処理を高速化すると共に、このイタレーションにおいては、高解像度の３次元データを使用することにより、精度を更に高めるものである。即ち、イタレーションにおいては、初期パラメータから推定される推定平面の十分近傍にある点をサンプルとして抽出する。

以下、このステップＳ４２及びＳ４３について更に詳細に説明する。図１１は、ステップＳ４２及びＳ４３を示すフローチャートである。図１１に示すように、初期パラメータ（θ_０，φ_０，ｄ_０）が入力されると、上述したステップＳ０３にて信頼度パラメータでフィルタ済みの入力３次元データセットの中から、誤差ｔｏｌで推定平面にのるデータポイント、即ち、下記数式（１８）の条件を満たすデータサブセットＰ_ｉを抽出する（ステップＳ４２１）。誤差ｔｏｌにおいて、誤差許容範囲の初期値ｔｏｌ_０は、予め適当に定めるものとする。

次に、下記数式（１９）に示すＰｉの一般化逆行列Ｐ_ｉ−１を計算し、最小二乗の意味でのデータに最適に当てはまる平面のパラメータΓ（α_ｉ，β_ｉ，γ_ｉ）＝（−１，−１，…，−１）Ｐ_ｉ−１を求める（ステップＳ４２２）。

そして、パラメータの変化量が十分小さく収束したとみなせるか、又はイタレーションの回数が予め設定された最大繰り返し数Ｉｔｅｒ_ｍａｘに達するかを判定し（ステップＳ４３１）、収束したか又は最大繰り返し数Ｉｔｅｒ_ｍａｘに達している場合はステップＳ４３２へ進む。また、誤差ｔｏｌを小さくさせて収束を早めることも可能である。一方、収束していない、又は所定回数に達していない場合は、次回のイタレーションにおける誤差許容値ｔｏｌ_ｉを誤差残差ｒ_ｉから決定し、ｉをインクリメントしてステップＳ４２１に戻る。

ステップＳ４３１から入力されたデータから下記数式（２０）によって平面パラメータ（θ，φ，ｄ）を決定する（ステップＳ３４２）。

図１０に戻って、ステップＳ４４では、ステップＳ４３から出力された平面パラメータ（θ，φ，ｄ）の信頼度を計算する。この信頼度の計算は、事前知識を有する場合は、ステップＳ４３２において算出された平面パラメータ（θ，φ，ｄ）と、事前知識とを比較し、予想の範囲内であることを確認し、範囲外である場合はこの値を棄却する。また、上述したステップＳ３におけるピーク値近傍の得票数又は得票値が夫々投票総数又は投票値の合計に占める割合、及びステップＳ４２のイタレーションにおいてパラメータ推定に使用した３次元データの個数等を推定結果の信頼度として使用することができる。即ち、信頼度は下記数式（２１−１）乃至（２１−３）に示す各指数によって計算することができる。ここで計算した信頼度は、平面パラメータと共に出力する。また、規定の信頼度に達しない場合は、そのデータを棄却する等することができる。

また、投票空間には、複数のピークが検出された場合には、各ピーク毎に上述したパラメータ推定処理（ステップＳ４２乃至Ｓ４４）を繰り返し、最も信頼度が高いパラメータを平面パラメータとして決定する。

更に、決定された平面パラメータ（θ，φ，ｄ）を有する平面について、ｚ’＝０とする座標変換行列を求め、入力データ群をその変換行列で写像して出力することにより、後述する後段においての処理が便利である。

本実施の形態においては、３次元データからハフ変換により直接平面パラメータを推定することができるため、ハフ変換の確率密度推定としての利点、即ち、ノイズに対してロバストなデータを得ることができ、また、平面パラメータを求める際は、ランダムにサンプリングすることができるため、より速く全体の統計的な特徴点を把握することができる。また、３次元パラメータの投票空間を上記数式（１−１）又は（１−２）と定義するため、より正確な投票結果を得ることができる。更に、ノイズが多い入力３次元データに対して信頼度パラメータを使用してデータの選別を行うと共に、この選別された３次元データから求められた３次元パラメータ自体の信頼性も考慮するため、ノイズに対して極めてロバストであり、信頼性及び安定性が高い結果を得ることができる。更にまた、投票を適当な回数でうち切り、イタレーション等により精度を向上することにより、ハフ変換により平面パラメータを求めても高速化が可能である。入力データをハードウェアで複数解像度でサンプリングするため、データ全体の大局的な構造をつかむフェーズでの高速化が可能となる。

また、ハフ変換は、投票先の３次元パラメータ空間の量子化サイズと精度との間にトレードオフの関係がある。即ち、量子化サイズを小さくすると、精度は向上するものの、サンプリング数を増やさなければならず、ピーク検出に時間がかかるが、量子化サイズを大きくすると、高速化するものの、結果の粒度が粗く精度が低下する。本実施の形態においては、投票結果のピーク周辺で重み付き平均処理をすることにより、投票先の３次元パラメータ空間の量子化サイズよりも細かい粒度のパラメータ推定が可能である。更に、ハフ変換の結果を初期パラメータとしてイタレーションによる最適化を実行して最終結果を生成するため、投票先の３次元パラメータ空間の量子化サイズをある程度の大きさとしても十分高い精度が得られる。

更に、ハフ変換においては、投票先の３次元パラメータ空間のサイズが大きくなると、処理速度が遅くなり、また記憶するデータ量が増加するため、カメラと平面との位置関係を別の手段で取得し、その情報を利用することでパラメータの検索空間を制限することにより、更に高速化が可能となる。このように、本実施の形態においては、ノイズにロバストであり精度が高いというハフ変換の利点を生かしつつ、高速化を実現することができる。

更にまた、３次元データ群から平面パラメータを推定する場合、その関係式は非線形の制約条件を含むため、１ステップの直接解放で解くことは難しいが、本実施の形態においては、カメラと平面との位置関係に対して、特殊な仮定、即ち、平面はカメラ位置（原点）を通らないという仮定をすることで、単なる線形問題に定式化し直し、この定式化を使用したことにより、極めて容易に平面方程式（平面パラメータ）が算出できる。

次に、以上説明した平面抽出部ＰＬＥＸ３２０を有する上述した図２に示すロボット装置１のソフトウェアの構成及び動作について詳細に説明する。図１２は、図２に示すソフトウェア３００の動作を示すフローチャートである。

図２に示すソフトウェア３００のキネマティックオドメトリＫＩＮＥ３１０には、上述したように、画像データ３０１及びセンサデータ３０２が入力される。この画像データ３０１は、カラー画像及びステレオカメラによる視差画像である。また、センサデータ３０２は、ロボット装置の関節角等のデータである。キネマティックオドメトリＫＩＮＥ３１０は、これらの入力データ３０１，３０２を受け取り、メモリに格納されていたそれまでの画像及びセンサデータを更新する（ステップＳ１０１）。

次に、これら画像データ３０１とセンサデータ３０２との時間的な対応を取る（ステップＳ１０２−１）。即ち、画像データ３０１の画像が撮像された時間におけるセンサデータ３０２の関節角を割り出す。次いで、この関節角のデータを使用してロボット装置１が中心に固定されたロボット中心座標系を頭部ユニットに設けられたカメラの座標系へ変換する（ステップＳ１０２−２）。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同時変換行列等を導出し、この同時変換行列３１１とこれに対応する画像データとを画像認識を行うオブジェクトへ送信する。即ち、同時変換行列３１１及びこれに対応する視差画像３１２を平面抽出部ＰＬＥＸ３２０へ出力し、同時変換行列３１１及びカラー画像３１３をランドマークセンサ部ＣＬＳ３４０へ出力する。

また、センサデータ３０２から得られる歩行パラメータと、足底センサを使用した歩数のカウント数とからロボット装置１の移動量を算出し、ロボット装置１のロボット装置中心座標系における移動量を算出する。以下、ロボット装置中心座標系の移動量をオドメトリともいう。このオドメトリ３１４を障害物グリッド算出部ＯＧ３３０及び絶対座標算出部ＬＺ３５０へ出力する。

平面抽出部ＰＬＥＸ３２０は、キネマティックオドメトリＫＩＮＥ３１０にて算出された同時変換行列３１１と、これに対応するステレオカメラから得られる視差画像３１２とが入力されると、それまでメモリに格納されていたこれらのデータを更新する（ステップＳ１０３）。そして、ステレオカメラのキャリブレーションパラメータ等を使用して視差画像３１２から３次元の位置データ（レンジデータ）を算出する（ステップＳ１０４−１）。次いで、このレンジデータからハフ変換等を使用して、例えば壁及びテーブル等の平面を除く平面を平面として抽出する。また、座標変換行列３１１からロボット装置１の足底が接地している平面との対応を取り、床面を選択し、床面上にない点、例えば所定の閾値よりも高い位置にあるもの等を障害物として床面からの距離を算出し、この障害物情報（obstacle）３２１を障害物グリッド算出部３３０に出力する（ステップS１０４−２）。

障害物グリッド算出部ＯＧ３３０では、上述したように、キネマティックオドメトリＫＩＮＥ３１０にて算出されたオドメトリ３１４と、平面抽出部ＰＬＥＸ３２０にて算出された障害物の観測情報（障害物情報）３２１とが入力されると、メモリに格納されていたそれまでのデータを更新する（ステップＳ１０５）。そして、床面上に障害物があるか否かの確率を保持する障害物グリッドを確率的手法により更新する（ステップＳ１０６）。

この障害物グリッド算出部ＯＧ３３０は、ロボット装置１を中心とした例えば周囲４ｍの障害物情報、即ち、上述した環境地図と、ロボット装置１の向く方角を示す姿勢情報とを保持しており、上述の方法により、環境地図を更新し、この更新した認識結果（障害物情報３３１）を出力することにより、上位レイヤ、即ち、本実施の形態においては、経路計画決定部ＳＢＬ３６０にて障害物を回避する計画を作成することができる。

ランドマークセンサＣＬＳ３４０は、キネマティックオドメトリＫＩＮＥ３１０から同時変換行列３１１及びカラー画像３１３が入力されると、予めメモリ内に格納されていたこれらのデータを更新する（ステップＳ１０７）。そして、カラー画像３１３の画像処理を行って、予め認識しているカラーランドマークを検出する。このカラーランドマークのカラー画像３１３上での位置及び大きさをカメラ座標系での位置に変換する。更に、同時変換行列３１１を使用し、カラーランドマークのカメラ座標系での位置をロボット中心位置座標系における位置に変換し、ロボット中心位置座標系におけるカラーランドマーク位置の情報（カラーランドマーク相対位置情報）３４１を絶対座標算出部ＬＺ３５０に出力する（ステップＳ１０８）。

絶対座標算出部ＬＺ３５０は、キネマティックオドメトリＫＩＮＥ３１０からのオドメトリ３１４と、ランドマークセンサ部ＣＬＳ３４０からのカラーランドマーク相対位置情報３４１とが入力されると、予めメモリ内に格納されていたこれらのデータが更新される（ステップＳ１０９）。そして、絶対座標算出部ＬＺ３５０が予め認識しているカラーランドマークの絶対座標（ワールド座標系での位置）、カラーランドマーク相対位置情報３４１、及びオドメトリ３１４を使用し、確率的手法によりロボット装置の絶対座標（ワールド座標系での位置）を算出する。そして、この絶対座標位置３５１を経路計画決定部ＳＢＬ３６０に出力する。

経路計画決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０から障害物グリッド情報３３１が入力され、絶対座標算出部ＬＺ３５０から絶対座標位置３５１が入力されると、予めメモリに格納されていたこれらのデータが更新される（ステップＳ１１１）。そして、経路計画決定部ＳＢＬ３６０障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する（ステップＳ１１２）。

また、人間によるナビゲーションの場合には、障害物グリッド算出部ＯＧ３３０からロボット装置の周囲に存在する障害物に関する認識結果及び絶対座標算出部ＬＺ３５０からの現在のロボット装置の位置の絶対座標をユーザに提供し、このユーザからの入力に応じて動作コマンドを発行させる。

図１３は、上述のソフトウェアに入力されるデータの流れを模式的に示す図である。なお、図１３において、図１及び図２に示す構成と同一構成要素には同一符号を伏してその詳細な説明は省略する。

顔検出部ＦＤＴ（ＦａｃｅＤｅｔｅｃｔｏｒ）３７１は、画像フレーム中から顔領域を検出するオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、それを例えば９段階のスケール画像に縮小変換する。この全ての画像の中から顔に相当する矩形領域を探索する。重なりあった候補領域を削減して最終的に顔と判断された領域に関する位置、大きさ、及び特徴量等の情報３７２を出力して、顔識別部ＦＩ（ＦａｃｅＩｄｅｎｔｉｆｙ）３７７へ送る。

顔識別部ＦＩ３７７は、検出された顔画像を識別するオブジェクトであり、顔の領域を示す矩形領域画像からなる情報３７２を顔検出部ＦＤＴ３７１から受け取り、この顔画像がメモリに格納されている手持ちの人物辞書のうちでどの人物に相当するかを比較して人物の識別を行う。そして、顔検出部ＦＤＴ３７１から受け取った顔画像の顔画像領域の位置、大きさ情報と共に人物のＩＤ情報３７８をＤＩＬ３７９に出力する。

色認識部ＭＣＴ（ＭｕｌｔｉＣｏｌｏｒＴｒａｃｋｅｒ）３７３は、色認識を行うオブジェクトであり、カメラ等の画像入力装置からカラー画像２０２を受け取り、予め保有している複数のカラー・モデル情報に基づいて色領域を抽出し、連続した領域に分割する。色認識部ＭＣＴ３７３は、分割した各領域の位置、大きさ及び特徴量等の情報３７４を距離情報付加部ＤＩＬ（ＤｉｓｔａｎｃｅＩｎｆｏｒｍａｔｉｏｎＬｉｎｋｅｒ）３７９に出力する。

動き検出部ＭＤＴ（ＭｏｔｉｏｎＤｅｔｅｃｔｏｒ）３７５は、画像の中で動いている部分を検出するオブジェクトであり、検出した動き領域の情報３７６を距離情報付加部ＤＩＬ３７９に出力する。

距離情報付加部ＤＩＬ３７９は、入力された２次元の情報に距離情報を付加して３次元の情報を出力するオブジェクトであり、顔検出部ＦＩ３７７からのＩＤ情報３７８、色認識部ＭＣＴ３７３からの分割した各領域の位置、大きさ及び特徴量等の情報３７４並びに動き検出部ＭＤＴ３７５からの動き領域の情報３７６に距離情報を付加して３次元情報３８０を短期記憶部ＳＴＭ（ＳｈｏｒｔＴｅｒｍＭｅｍｏｒｙ）３８１に出力する。

短期記憶部ＳＴＭ３８１は、ロボット装置１の外部環境に関する情報を比較的短い時間だけ保持するオブジェクトであり、ＡｒｔｈｕｒＤｅｃｏｄｅｒ（図示せず）から音声認識結果（単語、音源方向、確信度）を受け取り、色認識部ＭＣＴ３７３から肌色の領域の位置、大きさと顔領域の位置、大きさを受け取り、顔識別部ＦＩ３７７から人物のＩＤ情報等を受け取る。また、ロボット装置１の機体上の各センサからロボット装置の首の方向（関節角）を受け取る。そして、これらの認識結果及びセンサ出力を統合的に使って、現在どこにどの人物がいて、しゃべった言葉がどの人物のものであり、その人物とはこれまでにどんな対話を行ったのかという情報を保存する。こうした物体、即ちターゲットに関する物理情報と時間方向でみたイベント（履歴）を出力として、経路計画決定部（状況依存行動階層）（ＳＢＬ）３６０等の上位モジュールに渡す。

経路計画決定部ＳＢＬ３６０は、上述の短期記憶部ＳＴＭ３８１からの情報を基にロボット装置１の行動（状況に依存した行動）を決定するオブジェクトである。複数の行動を同時に評価したり、実行したりすることができる。また、行動を切り替えて機体をスリープ状態にしておき、別の行動を起動することができる。

以下、上述した各オブジェクトについて更に詳細に説明する。先ず、上述の実施の形態において説明した平面抽出部ＰＬＥＸ３２０について説明する。平面抽出部ＰＬＥＸ３２０は、同時変換行列３１１と、これに対応する図１４に示す左目画像２０１Ｌ及び右目画像２０１Ｒから得られた視差画像３１２を受け取り、以下の図１５に示す処理手順にしたがって障害物を認識する。

先ず、平面抽出部ＰＬＥＸ３２０は、同時変換行列３１１とこれに対応する視差画像３１２を受け取る（ステップＳ６１）。そして、視差画像３１２からレンズ歪みとステレオの取り付け誤差を吸収したキャリブレーションパラメータを利用して、画素毎にカメラ座標から見た３次元の位置データ（ｘ，ｙ，ｚ）を距離画像として生成する（ステップＳ６２）。

次に、平面抽出部ＰＬＥＸ３２０は、ステップＳ６２で生成された距離画像からハフ変換等を用いて画像中の支配的な平面のパラメータを検出する（ステップＳ６３）。この平面のパラメータは図１６に示すパラメータ空間（θ，φ）（投票空間）のヒストグラムにより検出される。パラメータの投票が小さいいと障害物、大きいと平面上のものと見なすことができる。この平面のパラメータを使って、元々画像から得られた距離の測定点が平面上からどれくらいの距離であるかが分かる。以上は、上述の実施の形態において説明した通りである。

次に、カメラ座標系の同時変換行列３１１から図１７に示すようにロボットの足底接地面への変換を求める（ステップＳ６４）。これにより、カメラ座標系で表現される接地平面のパラメータの算出が行われる。そして、前記ステップＳ６３での画像による平面の検出と、前記ステップＳ６４での足底接地平面の照合から、画像中の平面パラメータから床面に相当するものを選択する（ステップＳ６５）。

次に、前記ステップＳ６５にて選択された平面パラメータを使って、元の距離画像から平面上に載っている点を選択する（ステップＳ６６）。これは下記数式（２２）、数式（２３）を使用して、平面からの距離ｄがしきい値Ｄ_ｔｈより小さい事を使って判断する。

図１８には、しきい値Ｄ_ｔｈを１cmとした範囲で選択された測定点（×印）を示す。この図１８にて、黒く抜かれている点が平面と判断されなかった点である。

したがって、前記ステップＳ６６にて平面上（床面）上に載っている点以外の点（床面上に無い点）を障害物としてステップＳ６７にて認識することができる。これらの判断結果は床平面上の点（ｘ、ｙ）とその高さｚで表現される。例えば、高さｚ＜０は、平面より凹んでいる点を示す。

これにより、高さがロボットより高い障害物点はそれをくぐることができるので障害物ではないという判断も可能とすることができる。

また、ロボットビュー（図１９の（ａ））から得た床面抽出画像（図１９の（ｂ））の高さｚを０（ｚ＝０）となるように座標変換すれば、図１９の（ｃ）に示すように、平面上の２次元の位置で床か障害物かを表現することができる。

このように、障害物認識装置は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボットの姿勢から得られる床面パラメータと照合することで正しい平面を選択できる。また、障害物を認識するのではなく、実質的には床面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボットの大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、２次元床面上の障害物表現となるので経路計画などに既存の移動ロボットで用いられる手法が適用可能であるし、３次元の表現より高速に演算できる。

障害物グリッド算出部ＯＧ３３０は、所定の大きさのグリッドに分割された、ロボット中心座標系の地図情報である環境地図と、例えば環境地図上においてｘ軸方向又はｙ軸方向等の所定の方角からロボット装置が向く方角を示す姿勢情報とを有している。また、環境地図は、障害物情報が上述した平面抽出部ＰＬＥＸ３２０から入力され、障害物として認識されているグリッド（障害物占有領域）を有する。この障害物グリッド算出部ＯＧ３３０は、ロボット装置が移動すると、即ち、オドメトリ３１４が入力されると、予めメモリ内に格納されていた環境地図及び姿勢情報を、ロボット装置の姿勢の変化（差分移動角）及び移動距離（差分移動量）に伴って、予め認識している環境地図及び姿勢方向の情報を更新する。ここで、差分移動量がグリッドサイズより小さければ環境地図を更新せず、移動量がグリッドのサイズを超える場合は環境地図を更新する。また、環境地図及びそのグリッドの大きさを必要に応じて適宜変更することにより、計算量及びメモリコピーコスト等を低減することができる。

ランドマーク位置検出部ＣＬＳ３４０は、図２０に示すように、例えば、グリーン部分１００１、ピンク部分１００２、又はブルー部分１００３等のカラーを有する人工的なカラーランドマーク１００４を含む環境において、ロボット装置１のセンサ情報と、ロボット装置１が行った動作情報によって、ロボット装置の自己位置（位置及び姿勢）を同定する。

例えば、２次元的な作業空間上では、略等間隔状にグリッド（ｘ，ｙ）を設け、各グリッドの位置ｌ（localization）毎にロボット装置が存在する確率ｐ（ｌ）を管理する。この存在確率ｐ（ｌ）は、ロボット装置の移動（ｍｏｖｅ）、即ち内界観測情報ａ、又はランドマークの観測、即ち外界観測情報ｓの入力に応じて更新される。

存在確率ｐ（ｌ）は、ロボット装置の以前の状態、即ち自己位置ｌ'における存在確率ｐ（ｌ'）と、以前の状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）に依拠する。即ち、これまでの各状態ｌ'になる確率ｐ（ｌ'）と、状態ｌ'において移動ａを行ったときに状態ｌになるという遷移確率ｐ（ｌ｜ａ，ｌ'）の積を加算していく（若しくは積分する）ことによって、現在の状態、即ち自己位置ｌになるという存在確率ｐ（ｌ）に収束する。従って、外界観測結果としてのロボット装置の移動ａが観測されたときには、各グリッドでは、下記数式（２４）に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。

また、状態、即ち自己位置ｌにロボット装置が存在するという存在確率ｐ（ｌ）は、存在確率ｐ（ｌ）とこの状態ｌにおいてランドマークを観察するという遷移確率ｐ（ｓ｜ｌ）に依拠する。したがって、状態ｌにおいてランドマークの観測、即ち外界観測情報ｓが入力された場合には、下式に従ってロボット装置の存在確率ｐ（ｌ）を更新することができる。但し、下記数式（２５）に示すように右辺では、正規化のため、ランドマークを観察するという確率ｐ（ｓ）で除して正規化している。

図２１は、マルコフ・ローカリゼーションと拡張カルマン・フィルタを併用した本実施形態に係るランドマークセンサＣＬＳ（自己位置同定システム）３４０の機能構成を模式的に示している。同図に示すように、このランドマークセンサＣＬＳ３４０は、マルコフ・ローカリゼーション部（ＭＬ）３４２と、拡張カルマン・フィルタ部（ＥＫＬ）３４３と、拡張カルマン・フィルタ部３４３の動作を制御するＥＫＬ制御部３４４とで構成されている。

マルコフ・ローカリゼーション部３４２は、作業空間内の自分の位置を離散的なグリッド上の自己位置確率密度分布として保持し、ランドマークの観測に関する外界観測情報ｓと、ロボット装置自身の動作に関する内界観測情報ａを入力して、自己位置確率密度分布の更新を行う。そして、各時刻において、自己位置確率密度分布の最も高い値を持つグリッドを自己位置の推定結果としてＥＫＬ制御部３４４に出力する。

図２２は、マルコフ・ローカリゼーション部３４２により求められた各グリッド上での自己位置確率密度分布を表している。同図では、各グリッドにおける確率密度を濃淡により表現している。最も色が濃い、即ち自己位置確率密度分布が最も高い値を有するグリッドが自己位置の推定結果となる。

このマルコフ・ローカリゼーションによる自己位置同定は、センサのノイズに対してロバストであり、同定解の精度は粗いが解の収束速度が速いことを主な特徴とする。

一方、図２１に示す拡張カルマン・フィルタ部３４３は、自己位置を状態変数［ｘ，ｙ，θ］を実測値として保持し、環境内に設置されたカラーランドマーク１００４を観測すると、ランドマークからの相対位置に基づいて自己位置を推定する。また、自身の動作情報を観測すると、その動作情報に基づいて状態量の推定を行う。

拡張カルマン・フィルタ部３４３は、ロボット装置自身の動作情報ａと状態、即ち、自己位置ｌとの関係を規定した状態モデルと、自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルとで構成されている。

状態モデルは、ロボット装置が状態（自己位置）ｌにおいて動作ａを行った場合における理論的な状態ｌを与える遷移関数Ｆ（ｌ，ａ）を備えている。実際には理論的な状態ｌに対してノイズ成分ｗが重畳されることから、ロボット装置の状態ｌは状態モデルにより下記数式（２６）のように収束する。

また、観測モデルは、ロボット装置が状態、即ち自己位置ｉにおいて、ある既知の環境Ｅｎｖ（例えばランドマークの位置など）に関する観測の理論値ｓを与える観測関数Ｈ（Ｅｎｖ，ｌ）を備えている。実際には観測の理論値に対してノイズ成分ｖが重畳されることから、観測値ｓは観測モデルにより下記数式（２７）のように収束する。

なお、状態ｌ並びに観測ｓに重畳されるそれぞれのノイズｗ及びｖは、ここではゼロを中央値とするガウス分布として仮定する。

ロボット装置自身の動作情報ａと自己位置ｌとの関係を規定した状態モデルと自己位置ｌとランドマークの観測情報ｓとの関係を規定した観測モデルを備えた拡張カルマン・フィルタ部３４３において、動作情報ａは内界観測結果として、ランドマークの観測情報ｓは外界観測結果として、夫々既知である。従って、ロボット装置の自己位置同定を、ロボット装置の状態ｌをロボット装置の動作情報ａ及び観測情報ｓにより推定するという問題に帰着することができる。ここでは、ロボット装置の動作ａ、状態ｌ、及び観測ｓをそれぞれ下記に示すガウス分布として表現することができる。

ある時点でのロボット装置の状態ｌがある中央値と及び共分散を有するガウス分布をとるものとして推定する。そして、ロボット装置の動作ａが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（２８−１），（２８−２）により更新することができる。

ここで、▽Ｆ_ｌ，▽Ｆ_ａは、下記とする。

同様に、ある時点でのロボット装置の状態ｌがある中央値及び共分散を有するガウス分布をとるものとして推定する。そして、ランドマークの観測情報ｓが観測されたときには、状態ｌの推定値に関する中央値並びに共分散を下記数式（２９−１），（２９−２）により更新することができる。

ここで、各パラメータは下記数式（３０−１）乃至（３０−４）とする。

拡張カルマン・フィルタ３４３はセンサ情報に対するロバスト性に優れているので、拡張カルマン・フィルタ部３４３の推定結果は、ランドマークセンサＣＬＳ３４０全体の出力としている。

ＥＫＬ制御部３４４は、マルコフ・ローカリゼーション部３４２の出力結果に応じて拡張カルマン・フィルタ部３４４の動作を制御する。より具体的には、マルコフ・ローカリゼーション部３４２の自己位置推定結果に基づいて、ランドマークの観測情報ｓに関する妥当性を検証する。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈ_ｏｂｓを越えたかどうかで判断することができる。

グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈ_ｏｂｓを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度が高い解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。

また、ＥＫＬ制御部３４４は、拡張カルマン・フィルタ部３４３の自己位置推定結果の妥当性も検証する。自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。

分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３との各確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。

次に、ランドマークセンサＣＬＳ３４０の動作について説明する。図２３は、ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。図２３に示すように、ロボット装置１の移動に関する内界観測情報ａがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（２４）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０１）。次いで、拡張カルマン・フィルタ部３４３において、上記数式数（２８−１），（２８−２）を用いて自己位置推定値の更新処理が行われる（ステップＳ２０２）。

また、ランドマークの観測に関する外界観測情報ｓがランドマークセンサＣＬＳ３４０に入力されると、まずマルコフ・ローカリゼーション部３４２において、上記数式（２５）を用いて、自己位置推定値の更新処理が行われる（ステップＳ２１１）。

マルコフ・ローカリゼーション部３４２の出力結果は、ＥＫＬ制御部３４４に入力されて、観測情報ｓの妥当性が検証される（ステップＳ２１２）。観測情報ｓの妥当性は、マルコフ・ローカリゼーション部３４２において最大の存在確率となるグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が所定の閾値パラメータｔｈｒｅｓｈ_ｏｂｓを越えたかどうかで判断することができる。

グリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈ_ｏｂｓを下回る場合には、センサ・ノイズに対してロバストなマルコフ・ローカリゼーション部３４２においてさえ、センサ・ノイズのために同定解が充分に収束していないことが推測される。このような場合、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３において自己位置を推定しても、精度のよい解が得られず、むしろ演算時間を浪費するだけである。このため、観測情報ｓが妥当でないと判断された場合には、切換器３４５を用いて拡張カルマン・フィルタ部３４３への外界観測情報ｓ、即ちランドマークの観測情報の入力を遮断して、拡張カルマン・フィルタ部３４３における自己位置推定値の更新を停止させる。

他方、観測情報ｓを検証した結果、妥当性を満たす、即ちグリッド位置ｍｌｐにおいてランドマークを観測する確率ｐ（ｓ｜ｍｌｐ）が閾値パラメータｔｈｒｅｓｈ_ｏｂｓを上回る場合には、さらに拡張カルマン・フィルタ部３４３において、上記数式（２９−１），（２９−２）を用いて更新処理が行われる（ステップＳ２１３）。

拡張カルマン・フィルタ部３４３による自己位置推定の結果は、ＥＫＬ制御部３４４に入力されて、その妥当性が検証される（ステップＳ２１４）。拡張カルマン・フィルタ部３４３による自己位置推定結果の妥当性は、推定された状態ｌの中央値及び共分散を用いて、マルコフ・ローカリゼーション部３４２から出力される存在確率ｐ（ｌ）との分布比較テストにより判断することができる。分布比較テストの一例は、カイ自乗テストｃｈｉ−ｓｑｕａｒｅ−ｔｅｓｔ（ｍｌ，ｅｋｆ）である。

分布比較テストによりマルコフ・ローカリゼーション部３４２と拡張カルマン・フィルタ部３４３それぞれの確率分布が類似していない場合には、センサ・ノイズに対するロバスト性が低い拡張カルマン・フィルタ部３４３における自己位置推定値の方が、センサ・ノイズの影響により妥当でないと判断することができる。このような場合、ＥＫＬ制御部３４４は拡張カルマン・フィルタ部３４３の再初期化を行わせる（ステップＳ２１５）。何故ならば、拡張カルマン・フィルタは再復帰に多大な時間を要するからである。

このようにして、ランドマークセンサＣＬＳ３４０では、広い範囲で比較的短い探索時間で探索を行う大域探索と高精度だが探索時間を要する局所的探索とを併用して、高精度で、高速且つロバストな自己位置同定を行うことができる。

経路計画決定部ＳＢＬ３６０は、障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により、ロボット装置１の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部ＬＺ３５０から現在のロボット装置１の絶対座標を取得することにより、絶対座標系又はロボット装置中心のロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。即ち、入力データから状況に応じてロボット装置１が取るべき行動を決定し、その行動列を出力する。ここで、障害物グリッド算出部ＯＧ３３０の、障害物情報３３１に基づいた処理については後述する。

障害物グリッド算出部ＯＧ３３０からの障害物情報３３１により生成される障害物地図上の点は、図２４に示すように以下の３つの種類に分類される。

第１の点は、障害物の存在する点（図中の黒色で表される点）である。第２の点は、自由空間（障害物が存在しない空間）上の点（図中の白で表される点）である。そして、第３の点は、未観測領域上の点（図中の斜線で表される点）である。

次に、前記経路計画決定部ＳＢＬ３６０が採用する経路計画アルゴリズムを図２５のフローチャートに示し、以下に詳細を説明する。

先ず、現在位置から目的位置までを結ぶ直線経路の周辺の障害物地図が作成されるように、視線を目的地方向に向ける（ステップＳ７１）。そして距離画像を観測し、距離計測を行い、障害物地図を作成（更新）する（ステップＳ７２）。

次に、生成された障害物地図において、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行う（ステップＳ７３）。

この経路計画としては、例えば経路全体のコストを最小化するＡ^＊探索（Ａ^＊search)いう方法を用いる。このＡ^＊探索は、評価関数としてｆを用い、ｈ関数が許容的であるような最良優先探索である。いかなるヒューリスティック関数に対しても効率最適という点を用いている。

このステップＳ７３にて例えばＡ^＊探索を適用することにより生成された経路が移動可能であるな否かをステップＳ７４にてチェックし、障害物を回避することのできる経路が計画できなかった場合（ＮＯ）には、これ以上観測を続けても移動可能な経路が得られる可能性がないため、その旨を告げて経路計画を終了する（ステップＳ７５）。

ステップＳ７３にて移動可能経路が例えば前記Ａ^＊探索を適用して計画できた場合（ＹＥＳ）には、ステップＳ７６に進み、出力された経路上に未観測領域が含まれるか否かを検索する。このステップＳ７６にて経路上に未観測領域が含まれなかった場合（ＮＯ）には、ステップＳ７７にて目的地までの経路計画として移動可能経路を出力する。ステップＳ７６にて未観測領域が含まれる場合（ＹＥＳ）には、ステップＳ７８に進んで現在位置から未観測領域までの歩数を計算し、その歩数が閾値を超えるか否かをチェックする。

ステップＳ７８において歩数が閾値を超える場合（ＹＥＳ）には、ステップＳ７９により未知領域までの移動可能経路を出力してから、ステップＳ７１の処理に戻る。一方、ステップＳ７８において未知観測領域までの歩数が閾値に満たない場合（ＮＯ）は、未観測領域が距離観測されるように視線方向の制御を行って再観測を行い（ステップＳ８０）、再び障害物地図の更新を行う。

以上のような経路計画アルゴリズムを採用した経路計画決定部ＳＢＬ３６０は、未観測領域と自由空間領域を移動可能領域とみなして経路計画を行い、出力された経路上に含まれる未観測領域部のみを再観測することにより、目的地に移動する際に不必要な観測及び距離画像計算処理を行わなわず、効率的で短時間に移動経路計画を生成することが可能となる。

以下、本発明の実施の形態における上述した平面抽出装置を搭載した２足歩行タイプのロボット装置について詳細に説明する。この人間型のロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボットである。

図２６に示すように、ロボット装置１は、体幹部ユニット２の所定の位置に頭部ユニット３が連結されると共に、左右２つの腕部ユニット４Ｒ／Ｌと、左右２つの脚部ユニット５Ｒ／Ｌが連結されて構成されている（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。

このロボット装置１が具備する関節自由度構成を図２７に模式的に示す。頭部ユニット３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。

また、上肢を構成する各々の腕部ユニット４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール軸１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書ではゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。

また、体幹部ユニット２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。

また、下肢を構成する各々の脚部ユニット５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足部１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置１の股関節位置を定義する。人体の足部１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、ロボット装置１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。

以上を総括すれば、ロボット装置１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度、即ち関節数を適宜増減することができることはいうまでもない。

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

図２８には、ロボット装置１の制御システム構成を模式的に示している。同図に示すように、ロボット装置１は、ヒトの四肢を表現した体幹部ユニット２，頭部ユニット３，腕部ユニット４Ｒ／Ｌ，脚部ユニット５Ｒ／Ｌと、各ユニット間の協調動作を実現するための適応制御を行う制御ユニット１０とで構成される。

ロボット装置１全体の動作は、制御ユニット１０によって統括的に制御される。制御ユニット１０は、ＣＰＵ（Central Processing Unit）や、ＤＲＡＭ、フラッシュＲＯＭ等の主要回路コンポーネント（図示しない）で構成される主制御部１１と、電源回路やロボット装置１の各構成要素とのデータやコマンドの授受を行うインターフェイス（何れも図示しない）などを含んだ周辺回路１２とで構成される。

本発明を実現するうえで、この制御ユニット１０の設置場所は、特に限定されない。図２８では体幹部ユニット２に搭載されているが、頭部ユニット３に搭載してもよい。あるいは、ロボット装置１外に制御ユニット１０を配備して、ロボット装置１の機体とは有線又は無線で交信するようにしてもよい。

図２７に示したロボット装置１内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。即ち、頭部ユニット３には、首関節ヨー軸１０１、首関節ピッチ軸１０２、首関節ロール軸１０３の各々を表現する首関節ヨー軸アクチュエータＡ_２、首関節ピッチ軸アクチュエータＡ_３、首関節ロール軸アクチュエータＡ_４が配設されている。

また、頭部ユニット３には、外部の状況を撮像するためのＣＣＤ（Charge Coupled Device）カメラが設けられているほか、前方に位置する物体までの距離を測定するための距離センサ、外部音を集音するためのマイク、音声を出力するためのスピーカ、使用者からの「撫でる」や「叩く」といった物理的な働きかけにより受けた圧力を検出するためのタッチセンサ等が配設されている。

また、体幹部ユニット２には、体幹ピッチ軸１０４、体幹ロール軸１０５、体幹ヨー軸１０６の各々を表現する体幹ピッチ軸アクチュエータＡ_５、体幹ロール軸アクチュエータＡ_６、体幹ヨー軸アクチュエータＡ_７が配設されている。また、体幹部ユニット２には、このロボット装置１の起動電源となるバッテリを備えている。このバッテリは、充放電可能な電池によって構成されている。

また、腕部ユニット４Ｒ／Ｌは、上腕ユニット４_１Ｒ／Ｌと、肘関節ユニット４_２Ｒ／Ｌと、前腕ユニット４_３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸１０７、肩関節ロール軸１０８、上腕ヨー軸１０９、肘関節ピッチ軸１１０、前腕ヨー軸１１１、手首関節ピッチ軸１１２、手首関節ロール軸１１３の各々表現する肩関節ピッチ軸アクチュエータＡ_８、肩関節ロール軸アクチュエータＡ_９、上腕ヨー軸アクチュエータＡ_１０、肘関節ピッチ軸アクチュエータＡ_１１、肘関節ロール軸アクチュエータＡ_１２、手首関節ピッチ軸アクチュエータＡ_１３、手首関節ロール軸アクチュエータＡ_１４が配備されている。

また、脚部ユニット５Ｒ／Ｌは、大腿部ユニット５_１Ｒ／Ｌと、膝ユニット５_２Ｒ／Ｌと、脛部ユニット５_３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１５、股関節ピッチ軸１１６、股関節ロール軸１１７、膝関節ピッチ軸１１８、足首関節ピッチ軸１１９、足首関節ロール軸１２０の各々を表現する股関節ヨー軸アクチュエータＡ_１６、股関節ピッチ軸アクチュエータＡ_１７、股関節ロール軸アクチュエータＡ_１８、膝関節ピッチ軸アクチュエータＡ_１９、足首関節ピッチ軸アクチュエータＡ_２０、足首関節ロール軸アクチュエータＡ_２１が配備されている。各関節に用いられるアクチュエータＡ_２，Ａ_３・・・は、より好ましくは、ギア直結型で旦つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータで構成することができる。

体幹部ユニット２、頭部ユニット３、各腕部ユニット４Ｒ／Ｌ、各脚部ユニット５Ｒ／Ｌなどの各機構ユニット毎に、アクチュエータ駆動制御部の副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌが配備されている。さらに、各脚部ユニット５Ｒ／Ｌの足底が着床したか否かを検出する接地確認センサ３０Ｒ／Ｌを装着するとともに、体幹部ユニット２内には、姿勢を計測する姿勢センサ３１を装備している。

接地確認センサ３０Ｒ／Ｌは、例えば足底に設置された近接センサ又はマイクロ・スイッチなどで構成される。また、姿勢センサ３１は、例えば、加速度センサとジャイロ・センサの組み合わせによって構成される。

接地確認センサ３０Ｒ／Ｌの出力によって、歩行・走行などの動作期間中において、左右の各脚部が現在立脚又は遊脚何れの状態であるかを判別することができる。また、姿勢センサ３１の出力により、体幹部分の傾きや姿勢を検出することができる。

主制御部１１は、各センサ３０Ｒ／Ｌ，３１の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌの各々に対して適応的な制御を行い、ロボット装置１の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現できる。

ロボット装置１の機体上での全身運動は、足部運動、ＺＭＰ（Zero Moment Point）軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容にしたがった動作を指示するコマンドを各副制御部２０，２１，２２Ｒ／Ｌ，２３Ｒ／Ｌに転送する。そして、各々の副制御部２０，２１，・・・等では、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ_２，Ａ_３・・・等に対して駆動制御信号を出力する。ここでいう「ＺＭＰ」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ＺＭＰ軌道」とは、例えばロボット装置１の歩行動作期間中にＺＭＰが動く軌跡を意味する。なお、ＺＭＰの概念並びにＺＭＰを歩行ロボットの安定度判別規範に適用する点については、Miomir Vukobratovic著“LEGGED LOCOMOTION ROBOTS”（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））に記載されている。

以上のように、ロボット装置１は、各々の副制御部２０，２１，・・・等が、主制御部１１からの受信コマンドを解釈して、各アクチュエータＡ_２，Ａ_３・・・に対して駆動制御信号を出力し、各ユニットの駆動を制御している。これにより、ロボット装置１は、目標の姿勢に安定して遷移し、安定した姿勢で歩行できる。

また、ロボット装置１における制御ユニット１０では、上述したような姿勢制御のほかに、加速度センサ、タッチセンサ、接地確認センサ等の各種センサ、及びＣＣＤカメラからの画像情報、マイクからの音声情報等を統括して処理している。制御ユニット１０では、図示しないが加速度センサ、ジャイロ・センサ、タッチセンサ、距離センサ、マイク、スピーカなどの各種センサ、各アクチュエータ、ＣＣＤカメラ及びバッテリが各々対応するハブを介して主制御部１１と接続されている。

主制御部１１は、上述の各センサから供給されるセンサデータや画像データ及び音声データを順次取り込み、これらをそれぞれ内部インターフェイスを介してＤＲＡＭ内の所定位置に順次格納する。また、主制御部１１は、バッテリから供給されるバッテリ残量を表すバッテリ残量データを順次取り込み、これをＤＲＡＭ内の所定位置に格納する。ＤＲＡＭに格納された各センサデータ、画像データ、音声データ及びバッテリ残量データは、主制御部１１がこのロボット装置１の動作制御を行う際に利用される。

主制御部１１は、ロボット装置１の電源が投入された初期時、制御プログラムを読み出し、これをＤＲＡＭに格納する。また、主制御部１１は、上述のように主制御部１１よりＤＲＡＭに順次格納される各センサデータ、画像データ、音声データ及びバッテリ残量データに基づいて自己及び周囲の状況や、使用者からの指示及び働きかけの有無などを判断する。

さらに、主制御部１１は、この判断結果及びＤＲＡＭに格納した制御プログラムに基づいて自己の状況に応じて行動を決定するとともに、当該決定結果に基づいて必要なアクチュエータを駆動させることによりロボット装置１に、いわゆる「身振り」、「手振り」といった行動をとらせる。

このようにしてロボット装置１は、制御プログラムに基づいて自己及び周囲の状況を判断し、使用者からの指示及び働きかけに応じて自律的に行動できる。

ところで、このロボット装置１は、内部状態に応じて自律的に行動することができる。そこで、ロボット装置１における制御プログラムのソフトウェア構成例について、図２９乃至図３４を用いて説明する。

図２９において、デバイス・ドライバ・レイヤ４０は、制御プログラムの最下位層に位置し、複数のデバイス・ドライバからなるデバイス・ドライバ・セット４１から構成されている。この場合、各デバイス・ドライバは、ＣＣＤカメラやタイマ等の通常のコンピュータで用いられるハードウェアに直接アクセスすることを許されたオブジェクトであり、対応するハードウェアからの割り込みを受けて処理を行う。

また、ロボティック・サーバ・オブジェクト４２は、デバイス・ドライバ・レイヤ４０の最下位層に位置し、例えば上述の各種センサやアクチュエータ２８_１〜２８_ｎ等のハードウェアにアクセスするためのインターフェイスを提供するソフトウェア群でなるバーチャル・ロボット４３と、電源の切換えなどを管理するソフトウェア群でなるパワーマネージャ４４と、他の種々のデバイス・ドライバを管理するソフトウェア群でなるデバイス・ドライバ・マネージャ４５と、ロボット装置１の機構を管理するソフトウェア群でなるデザインド・ロボット４６とから構成されている。

マネージャ・オブジェクト４７は、オブジェクト・マネージャ４８及びサービス・マネージャ４９から構成されている。オブジェクト・マネージャ４８は、ロボティック・サーバ・オブジェクト４２、ミドル・ウェア・レイヤ５０、及びアプリケーション・レイヤ５１に含まれる各ソフトウェア群の起動や終了を管理するソフトウェア群であり、サービス・マネージャ４９は、メモリカードに格納されたコネクションファイルに記述されている各オブジェクト間の接続情報に基づいて各オブジェクトの接続を管理するソフトウェア群である。

ミドル・ウェア・レイヤ５０は、ロボティック・サーバ・オブジェクト４２の上位層に位置し、画像処理や音声処理などのこのロボット装置１の基本的な機能を提供するソフトウェア群から構成されている。また、アプリケーション・レイヤ５１は、ミドル・ウェア・レイヤ５０の上位層に位置し、当該ミドル・ウェア・レイヤ５０を構成する各ソフトウェア群によって処理された処理結果に基づいてロボット装置１の行動を決定するためのソフトウェア群から構成されている。

なお、ミドル・ウェア・レイヤ５０及びアプリケーション・レイヤ５１の具体なソフトウェア構成をそれぞれ図３０に示す。

ミドル・ウェア・レイヤ５０は、図３０に示すように、騒音検出用、温度検出用、明るさ検出用、音階認識用、距離検出用、姿勢検出用、タッチセンサ用、動き検出用及び色認識用の各信号処理モジュール６０〜６８並びに入力セマンティクスコンバータモジュール６９などを有する認識系７０と、出力セマンティクスコンバータモジュール７８並びに姿勢管理用、トラッキング用、モーション再生用、歩行用、転倒復帰用、ＬＥＤ点灯用及び音再生用の各信号処理モジュール７１〜７７などを有する出力系７９とから構成されている。

認識系７０の各信号処理モジュール６０〜６８は、ロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３によりＤＲＡＭから読み出される各センサデータや画像データ及び音声データのうちの対応するデータを取り込み、当該データに基づいて所定の処理を施して、処理結果を入力セマンティクスコンバータモジュール６９に与える。ここで、例えば、バーチャル・ロボット４３は、所定の通信規約によって、信号の授受或いは変換をする部分として構成されている。

入力セマンティクスコンバータモジュール６９は、これら各信号処理モジュール６０〜６８から与えられる処理結果に基づいて、「うるさい」、「暑い」、「明るい」、「ボールを検出した」、「転倒を検出した」、「撫でられた」、「叩かれた」、「ドミソの音階が聞こえた」、「動く物体を検出した」又は「障害物を検出した」などの自己及び周囲の状況や、使用者からの指令及び働きかけを認識し、認識結果をアプリケーション・レイヤ４１に出力する。

アプリケーション・レイヤ５１は、図３１に示すように、行動モデルライブラリ８０、行動切換モジュール８１、学習モジュール８２、感情モデル８３及び本能モデル８４の５つのモジュールから構成されている。

行動モデルライブラリ８０には、図３２に示すように、「バッテリ残量が少なくなった場合」、「転倒復帰する」、「障害物を回避する場合」、「感情を表現する場合」、「ボールを検出した場合」などの予め選択されたいくつかの条件項目にそれぞれ対応させて、それぞれ独立した行動モデルが設けられている。

そして、これら行動モデルは、それぞれ入力セマンティクスコンバータモジュール６９から認識結果が与えられたときや、最後の認識結果が与えられてから一定時間が経過したときなどに、必要に応じて後述のように感情モデル８３に保持されている対応する情動のパラメータ値や、本能モデル８４に保持されている対応する欲求のパラメータ値を参照しながら続く行動をそれぞれ決定し、決定結果を行動切換モジュール８１に出力する。

なお、この実施の形態の場合、各行動モデルは、次の行動を決定する手法として、図３３に示すような１つのノード（状態）ＮＯＤＥ_０〜ＮＯＤＥ_ｎから他のどのノードＮＯＤＥ_０〜ＮＯＤＥ_ｎに遷移するかを各ノードＮＯＤＥ_０〜ＮＯＤＥ_ｎに間を接続するアークＡＲＣ_１〜ＡＲＣ_ｎ１に対してそれぞれ設定された遷移確率Ｐ_１〜Ｐ_ｎに基づいて確率的に決定する有限確率オートマトンと呼ばれるアルゴリズムを用いる。

具体的に、各行動モデルは、それぞれ自己の行動モデルを形成するノードＮＯＤＥ_０〜ＮＯＤＥ_ｎにそれぞれ対応させて、これらノードＮＯＤＥ_０〜ＮＯＤＥ_ｎ毎に図３４に示すような状態遷移表９０を有している。

この状態遷移表９０では、そのノードＮＯＤＥ_０〜ＮＯＤＥ_ｎにおいて遷移条件とする入力イベント（認識結果）が「入力イベント名」の列に優先順に列記され、その遷移条件についてのさらなる条件が「データ名」及び「データ範囲」の列における対応する行に記述されている。

従って、図３４の状態遷移表９０で表されるノードＮＯＤＥ_１００では、「ボールを検出（ＢＡＬＬ）」という認識結果が与えられた場合に、当該認識結果とともに与えられるそのボールの「大きさ（SIZE）」が「0から1000」の範囲であることや、「障害物を検出（OBSTACLE）」という認識結果が与えられた場合に、当該認識結果とともに与えられるその障害物までの「距離（DISTANCE）」が「0から100」の範囲であることが他のノードに遷移するための条件となっている。

また、このノードＮＯＤＥ_１００では、認識結果の入力がない場合においても、行動モデルが周期的に参照する感情モデル８３及び本能モデル８４にそれぞれ保持された各情動及び各欲求のパラメータ値のうち、感情モデル８３に保持された「喜び（Joy）」、「驚き（Surprise）」又は「悲しみ（Sadness）」の何れかのパラメータ値が「50から100」の範囲であるときには他のノードに遷移することができるようになっている。

また、状態遷移表９０では、「他のノードヘの遷移確率」の欄における「遷移先ノード」の行にそのノードＮＯＤＥ_０〜ＮＯＤＥ_ｎから遷移できるノード名が列記されているとともに、「入力イベント名」、「データ名」及び「データの範囲」の列に記述された全ての条件が揃ったときに遷移できる他の各ノードＮＯＤＥ_０〜ＮＯＤＥ_ｎへの遷移確率が「他のノードヘの遷移確率」の欄内の対応する箇所にそれぞれ記述され、そのノードＮＯＤＥ_０〜ＮＯＤＥ_ｎに遷移する際に出力すべき行動が「他のノードヘの遷移確率」の欄における「出力行動」の行に記述されている。なお、「他のノードヘの遷移確率」の欄における各行の確率の和は１００［％］となっている。

従って、図３４の状態遷移表９０で表されるノードＮＯＤＥ_１００では、例えば「ボールを検出（ＢＡＬＬ）」し、そのボールの「ＳＩＺＥ（大きさ）」が「0から1000」の範囲であるという認識結果が与えられた場合には、「30［％］」の確率で「ノードＮＯＤＥ_１２０（node 120）」に遷移でき、そのとき「ＡＣＴＩＯＮ１」の行動が出力されることとなる。

各行動モデルは、それぞれこのような状態遷移表９０として記述されたノードＮＯＤＥ_０〜 ＮＯＤＥ_ｎが幾つも繋がるようにして構成されており、入力セマンティクスコンバータモジュール６９から認識結果が与えられたときなどに、対応するノードＮＯＤＥ_０〜ＮＯＤＥ_ｎの状態遷移表を利用して確率的に次の行動を決定し、決定結果を行動切換モジュール８１に出力するようになされている。

図３１に示す行動切換モジュール８１は、行動モデルライブラリ８０の各行動モデルからそれぞれ出力される行動のうち、予め定められた優先順位の高い行動モデルから出力された行動を選択し、当該行動を実行すべき旨のコマンド（以下、行動コマンドという。）をミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８に送出する。なお、この実施の形態においては、図３２において下側に表記された行動モデルほど優先順位が高く設定されている。

また、行動切換モジュール８１は、行動完了後に出力セマンティクスコンバータモジュール７８から与えられる行動完了情報に基づいて、その行動が完了したことを学習モジュール８２、感情モデル８３及び本能モデル８４に通知する。

一方、学習モジュール８２は、入力セマンティクスコンバータモジュール６９から与えられる認識結果のうち、「叩かれた」や「撫でられた」など、使用者からの働きかけとして受けた教示の認識結果を入力する。

そして、学習モジュール８２は、この認識結果及び行動切換えモジュール７１からの通知に基づいて、「叩かれた（叱られた）」ときにはその行動の発現確率を低下させ、「撫でられた（誉められた）」ときにはその行動の発現確率を上昇させるように、行動モデルライブラリ７０における対応する行動モデルの対応する遷移確率を変更する。

他方、感情モデル８３は、「喜び（Joy）」、「悲しみ（Sadness）」、「怒り（Anger）」、「驚き（Surprise）」、「嫌悪（Disgust）」及び「恐れ（Fear）」の合計６つの情動について、各情動毎にその情動の強さを表すパラメータを保持している。そして、感情モデル８３は、これら各情動のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる「叩かれた」及び「撫でられた」などの特定の認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。

具体的には、感情モデル８３は、入力セマンティクスコンバータモジュール６９から与えられる認識結果と、そのときのロボット装置１の行動と、前回更新してからの経過時間となどに基づいて所定の演算式により算出されるそのときのその情動の変動量を△Ｅ［ｔ］、現在のその情動のパラメータ値をＥ［ｔ］、その情動の感度を表す係数をｋ_ｅとして、下記数式（３１）によって次の周期におけるその情動のパラメータ値Ｅ［ｔ＋１］を算出し、これを現在のその情動のパラメータ値Ｅ［ｔ］と置き換えるようにしてその情動のパラメータ値を更新する。また、感情モデル８３は、これと同様にして全ての情動のパラメータ値をにより更新する。

なお、各認識結果や出力セマンティクスコンバータモジュール７８からの通知が各情動のパラメータ値の変動量△Ｅ［ｔ］にどの程度の影響を与えるかは予め決められており、例えば「叩かれた」といった認識結果は「怒り」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与え、「撫でられた」といった認識結果は「喜び」の情動のパラメータ値の変動量△Ｅ［ｔ］に大きな影響を与えるようになっている。

ここで、出力セマンティクスコンバータモジュール７８からの通知とは、いわゆる行動のフィードバック情報（行動完了情報）であり、行動の出現結果の情報であり、感情モデル８３は、このような情報によっても感情を変化させる。これは、例えば、「叫ぶ」といった行動により怒りの感情レベルが下がるといったようなことである。なお、出力セマンティクスコンバータモジュール７８からの通知は、上述した学習モジュール８２にも入力されており、学習モジュール８２は、その通知に基づいて行動モデルの対応する遷移確率を変更する。

なお、行動結果のフィードバックは、行動切換モジュール８１の出力（感情が付加された行動）によりなされるものであってもよい。

一方、本能モデル８４は、「運動欲（exercise）」、「愛情欲（affection）」、「食欲（appetite）」及び「好奇心（curiosity）」の互いに独立した４つの欲求について、これら欲求毎にその欲求の強さを表すパラメータを保持している。そして、本能モデル８４は、これらの欲求のパラメータ値を、それぞれ入力セマンティクスコンバータモジュール６９から与えられる認識結果や、経過時間及び行動切換モジュール８１からの通知などに基づいて周期的に更新する。

具体的には、本能モデル８４は、「運動欲」、「愛情欲」及び「好奇心」については、認識結果、経過時間及び出力セマンティクスコンバータモジュール７８からの通知などに基づいて所定の演算式により算出されるそのときのその欲求の変動量をΔＩ［ｋ］、現在のその欲求のパラメータ値をＩ［ｋ］、その欲求の感度を表す係数ｋ_ｉとして、所定周期で下記数式（３２）を用いて次の周期におけるその欲求のパラメータ値Ｉ［ｋ＋１］を算出し、この演算結果を現在のその欲求のパラメータ値Ｉ［ｋ］と置き換えるようにしてその欲求のパラメータ値を更新する。また、本能モデル８４は、これと同様にして「食欲」を除く各欲求のパラメータ値を更新する。

なお、認識結果及び出力セマンティクスコンバータモジュール７８からの通知などが各欲求のパラメータ値の変動量△Ｉ［ｋ］にどの程度の影響を与えるかは予め決められており、例えば出力セマンティクスコンバータモジュール７８からの通知は、「疲れ」のパラメータ値の変動量△Ｉ［ｋ］に大きな影響を与えるようになっている。

なお、本実施の形態においては、各情動及び各欲求（本能）のパラメータ値がそれぞれ０から１００までの範囲で変動するように規制されており、また係数ｋ_ｅ、ｋ_ｉの値も各情動及び各欲求毎に個別に設定されている。

一方、ミドル・ウェア・レイヤ５０の出力セマンティクスコンバータモジュール７８は、図３０に示すように、上述のようにしてアプリケーション・レイヤ５１の行動切換モジュール８１から与えられる「前進」、「喜ぶ」、「鳴く」又は「トラッキング（ボールを追いかける）」といった抽象的な行動コマンドを出力系７９の対応する信号処理モジュール７１〜７７に与える。

そしてこれら信号処理モジュール７１〜７７は、行動コマンドが与えられると当該行動コマンドに基づいて、その行動をするために対応するアクチュエータに与えるべきサーボ指令値や、スピーカから出力する音の音声データ及び又はＬＥＤに与える駆動データを生成し、これらのデータをロボティック・サーバ・オブジェクト４２のバーチャル・ロボット４３及び信号処理回路を順次介して対応するアクチュエータ又はスピーカ又はＬＥＤに順次送出する。

このようにしてロボット装置１は、上述した制御プログラムに基づいて、自己（内部）及び周囲（外部）の状況や、使用者からの指示及び働きかけに応じた自律的な行動ができる。

このような制御プログラムは、ロボット装置が読取可能な形式で記録された記録媒体を介して提供される。制御プログラムを記録する記録媒体としては、磁気読取方式の記録媒体（例えば、磁気テープ、フレキシブルディスク、磁気カード）、光学読取方式の記録媒体（例えば、ＣＤ−ＲＯＭ、ＭＯ、ＣＤ−Ｒ、ＤＶＤ）等が考えられる。記録媒体には、半導体メモリ（いわゆるメモリカード（矩形型、正方形型など形状は問わない。）、ＩＣカード）等の記憶媒体も含まれる。また、制御プログラムは、いわゆるインターネット等を介して提供されてもよい。

これらの制御プログラムは、専用の読込ドライバ装置、又はパーソナルコンピュータ等を介して再生され、有線又は無線接続によってロボット装置１に伝送されて読み込まれる。また、ロボット装置１は、半導体メモリ、又はＩＣカード等の小型化された記憶媒体のドライブ装置を備える場合、これら記憶媒体から制御プログラムを直接読み込むこともできる。

本実施の形態においては、頭部ユニットのＣＣＤカメラ２００Ｒ，２００Ｌ及び画像処理回路２１０からなるステレオ距離計測装置から信頼度画像を使用して求められた正確な補正距離データが情報処理手段に入力されるため、ロボット装置は、ステレオ距離計測データを用いた位置認識及び物体認識等、ロボットの後段の認識精度を高めることができる。

また、上記パラメータは、ＣＣＤカメラのパラメータと、ロボット装置のパラメータとを含み、信頼度画像の信頼度に基づいてカメラパラメータ及びロボットパラメータを制御することで、ＣＣＤ画像の撮像条件及び撮像位置等のカメラパラメータを制御するだけではなく、ロボット装置の頭部ユニット、左右２つの腕部ユニット４Ｒ／Ｌ及び左右２つの脚部ユニット５Ｒ／Ｌ等を動作させるロボットパラメータを種々変更することにより、更に信頼度が高い距離画像を得ることができ、ロボット装置の性能が向上する。

このようなロボット装置によれば、供給された入力情報に基づいて動作を行う自律型のロボット装置であって、３次元データ群から３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出手段と、この平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定手段とを有するため、高い精度で平面を抽出することができ、この平面検出結果から障害物等のロボット装置の環境を認識することができる。

以上詳細に説明したように本発明に係る平面抽出方法の実施の形態によれば、３次元データ群から３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、この平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有するため、平面パラメータを投票空間に直接投票することができるため、ノイズに対してロバストで、精度が高く、従って、サンプリング数を減らして処理を高速化することができる。

本発明の実施の形態におけるロボット装置の概略を示すブロック図である。 本発明の実施の形態におけるロボット装置のソフトウェアの構成を示す模式図である。 本発明の実施の形態における平面抽出方法を示すフローチャートである。 図３に示すステップＳ１の工程を説明するフローチャートである。 図３に示すステップＳ２及びＳ３の工程を説明するフローチャートである。 本発明の実施の形態における平面パラメータを示す模式図である。 ３次元パラメータ投票空間を示す模式図である。 本発明の実施の形態における３次元パラメータ投票空間を示す模式図である。 図３に示すステップＳ４の工程を説明するフローチャートである。 同じく、図３に示すステップＳ４の工程を示すフローチャートである。 図９に示すステップＳ４２及びＳ４３の工程を示すフローチャートである。 本発明の実施の形態におけるロボット装置のソフトウェアの動作を示すフローチャートである。 同ソフトウェアに入力されるデータの流れを示す模式図である。 平面抽出部ＰＬＥＸに入力される視差画像の生成を説明するための図である。 平面抽出部ＰＬＥＸが障害物を認識する処理手順を示すフローチャートである。 平面検出部ＰＬＥＸが検出する平面パラメータを示す図である。 カメラ座標系から足底接地平面座標系への変換処理を説明するための図である。 平面抽出部ＰＬＥＸにて抽出された平面上の点を示す図である。 ロボットビューから床面を抽出し、更に座標変換して２次元（床平面）乗に障害物を表現したことを示す図である。 ランドマークセンサＣＬＳにおけるカラーランドマークを示す模式図である。 ランドマークセンサＣＬＳの構成を模式的に示すブロック図である。 マルコフ・ローカリゼーション部により求められた各グリッド上での自己位置確率密度分布を示す模式図である。 ランドマークセンサＣＬＳ３４０の動作を示すフローチャートである。 障害物情報により生成された障害物地図上に、現在位置から目的位置までの経路を示す模式図である。 経路計画アルゴリズムを示すフローチャートである。 本発明の実施の形態におけるロボット装置の外観構成を示す斜視図である。 同ロボット装置の自由度構成モデルを模式的に示す図である。 同ロボット装置の回路構成を示すブロック図である。 同ロボット装置のソフトウェア構成を示すブロック図である。 同ロボット装置のソフトウェア構成におけるミドル・ウェア・レイヤの構成を示すブロック図である。 同ロボット装置のソフトウェア構成におけるアプリケーション・レイヤの構成を示すブロック図である。 アプリケーション・レイヤの行動モデルライブラリの構成を示すブロック図である。 同ロボット装置の行動決定のための情報となる有限確率オートマトンを説明する図である。 有限確率オートマトンの各ノードに用意された状態遷移表を示す図である。

符号の説明

１ ロボット装置、２５０ 頭部ユニット、２００Ｒ，２００Ｌ ＣＣＤカメラ、２０１Ｒ 右目画像、２０１Ｌ 左目画像、２０２ カラー画像、２０３ 視差画像、２１０ ステレオ画像処理装置、２２０ ＣＰＵ、２３０ アクチュエータ、２３１ 制御信号、２４０ センサ、２４１ センサデータ、２６０ 体幹部、３００ ソフトウェア、３１０ キネマティックオドメトリＫＩＮＥ、３２０ 平面抽出部ＰＬＥＸ、３３０ 障害物グリッド算出部ＯＧ、３４０ ランドマーク位置検出部ＣＬＳ、３５０ 絶対座標算出部ＬＺ、３６０ 行動決定部ＳＢＬ、４００，５００，５１０，６００，６１０，７００

Claims (15)

1. 画像処理手段が、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、
   制御手段が、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、
   制御手段が、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、
   制御手段が、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、
   上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなる
   ことを特徴とする平面抽出方法。
2. 画像処理手段が、入力画像を対応する３次元データ群に変換する工程と、
   制御手段が、上記３次元データ群から３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、
   制御手段が、上記平面算出工程から得られた複数の平面パラメータを、上記３次元データの信頼度により異なる重みを定めて投票空間に重付投票してこの投票結果に基づき平面を決定する平面決定工程と
   を有することを特徴とする請求項１記載の平面抽出方法。
3. 画像処理手段が、入力画像を対応する３次元データ群に変換する工程と、
   制御手段が、上記３次元データ群から３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、
   制御手段が、上記平面算出工程から得られた複数の平面パラメータを、平面算出方法により異なる重みを定めて投票空間に重付投票してこの投票結果に基づき平面を決定する平面決定工程と
   を有することを特徴とする請求項１記載の平面抽出方法。
4. 上記平面算出工程は、上記平面の法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたとき、上記平面パラメータ（θ，φ，ｄ）を算出する工程であって、
   上記投票空間は、下記数式（１−１）又は数式（１−２）で表されるものである
   ことを特徴とする請求項１記載の平面抽出方法。
   

   
5. 上記サンプリングデータは、上記３次元データ群を有する３次元空間を所定の領域に分割し、この分割された領域に含まれる３次元データとすることを特徴とする請求項１記載の平面抽出方法。
6. 上記平面算出工程は、３点以上の上記３次元データから上記平面が原点を通らない任意の平面であると仮定し最小二乗法を使用して上記平面パラメータを算出する工程を有することを特徴とする請求項１記載の平面抽出方法。
7. 上記平面決定工程は、上記投票を繰り返し、上記投票の総数が所定の閾値を超えた場合、又は上記投票数に基づいて決定される投票値が最も高い位置における投票値の合計が全ての投票値の合計である総投票値の所定の割合に達した場合に投票をうち切る工程を有することを特徴とする請求項１記載の平面抽出方法。
8. 上記平面決定工程は、上記投票空間の投票のピーク近傍における投票数による重み付き平均を行う重付平均工程を有することを特徴とする請求項１記載の平面抽出方法。
9. 上記平面決定工程は、上記投票結果により推定された平面パラメータを初期パラメータとしてイタレーションにより該初期パラメータから最適パラメータを算出する最適パラメータ算出工程を有する請求項１記載の平面抽出方法。
10. 上記平面決定工程は、上記投票における投票ピークの鋭さ、上記イタレーションに使用したデータポイント数、及び上記イタレーションにより算出される誤差残差からなる群から選択された１以上を信頼度のパラメータとして使用し、決定される平面の信頼度を算出する信頼度算出工程を有することを特徴とする請求項９記載の平面抽出方法。
11. 上記パラメータ算出工程は、上記初期パラメータを推定する平面算出工程における上記３次元データより、解像度が高い３次元データを使用してイタレーションを行うことを特徴とする請求項１０記載の平面抽出方法。
12. 入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する画像処理手段と、
    上記３次元データ群から、上記画像処理手段において算出された上記信頼度パラメータに基づいて、上記３次元データを選別するデータ選別手段と、
    上記データ選別手段にて選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出手段と、
    この平面算出手段から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定手段とを有し、
    上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなる
    ことを特徴とする平面抽出装置。
13. 所定の動作をコンピュータに実行させるためのプログラムであって、
    コンピュータが、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、
    コンピュータが、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、
    コンピュータが、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、
    コンピュータが、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、
    上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなる
    ことを特徴とするプログラム。
14. 所定の動作をコンピュータに実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体であって、
    コンピュータが、入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する信頼度パラメータ算出工程と、
    コンピュータが、上記３次元データ群から、上記信頼度パラメータ算出工程において算出された上記信頼度パラメータに基づいて、３次元データを選別するデータ選別工程と、
    コンピュータが、上記データ選別工程において選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出工程と、
    コンピュータが、上記平面算出工程から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定工程とを有し、
    上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなる
    ことを特徴とするプログラムを記録した記録媒体。
15. 外部の状況を撮像するための撮像手段と、
    上記撮像手段からの入力画像を対応する３次元データ群に変換し、変換された３次元データ群のそれぞれの３次元データに対応する信頼度パラメータを算出する画像処理手段と、
    上記３次元データ群から、上記画像処理手段において算出された上記信頼度パラメータに基づいて、上記３次元データを選別するデータ選別手段と、
    上記データ選別手段にて選別された上記３次元データから、３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出手段と、
    この平面算出手段から得られた複数の平面パラメータを投票空間に投票してこの投票結果に基づき平面を決定する平面決定手段とを有し、
    上記サンプリングデータは、ランダムにサンプリングされた１つの基準点と、この基準点から所定の距離内でランダムにサンプリングされた他の２点とからなる
    ことを特徴とする撮像装置。

Images