JP2003271975A - 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置 - Google Patents

Abstract

(57)【要約】 【課題】 ノイズに対してロバストであり、信頼性が高
い平面を抽出することができる平面抽出方法、その装
置、そのプログラム、その記録媒体及び平面抽出装置搭
載型ロボット装置を提供する。 【解決手段】 距離画像等の３次元データから平面を抽
出するには、先ず、３次元データ群から３次元データを
ランダムにサンプリングし、法線ベクトルの向きを
（θ，φ）、原点からの距離をｄとしたときの平面パラ
メータ（θ，φ，ｄ）を算出して、平面パラメータを投
票空間（θ，Ψ，ｄ）＝（θ，φｃｏｓθ，ｄ）に投票
する。サンプルデータの信頼度及びパラメータの算出方
法により投票値に重み付けする。次に、得票値が高くな
ると推定された平面パラメータのイタレーション処理を
して平面パラメータを決定する。最後に、サンプリング
データの信頼度、サンプリング数、イタレーション時の
誤差等から得られた信頼度と共に平面パラメータを出力
する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

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

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

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

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

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

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

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

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

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

【００１４】更に、特開平７−２７１９７８号公報に
は、メモリ容量及び処理時間短縮を図った画像処理装置
が開示されている（以下、従来例３という。）この従来
例３に記載の技術においては、３次元空間から直線群を
入力し、この直線群の法線ベクトルを特徴空間に投票
し、これに基づき所定の本数の直線を含む平面を抽出す
るものである。

【００１５】

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

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

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

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

【００１９】

【課題を解決するための手段】上述した目的を達成する
ために、本発明に係る平面抽出方法は、３次元データ群
から３点以上の３次元データをサンプリングし、上記３
次元データによって決まる１つの平面を示す平面パラメ
ータを複数算出する平面算出工程と、この平面算出工程
から得られた複数の平面パラメータを投票空間に投票し
てこの投票結果に基づき平面を決定する平面決定工程と
を有することを特徴とする。

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

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

【００２２】

【数３】

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

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

【００２５】更に、上記サンプリングデータは、３点を
ランダムにサンプリングしたものか、又はランダムにサ
ンプリングされた１つの基準点と、この基準点から所定
の距離内でランダムにサンプリングされた他の２点とか
らなるものであって、上記ランダムサンプリングを複数
回繰り返すことができる。更にまた、３点のデータから
直接解法によって上記平面パラメータを算出する工程を
有することができ、サンプリングデータから上記投票空
間へ投票するハフ変換により高精度の平面パラメータを
推定することができる。

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

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

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

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

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

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

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

【００３３】本発明に係る平面抽出装置は、３次元デー
タ群から３点以上の３次元データをサンプリングし、上
記３次元データによって決まる１つの平面を示す平面パ
ラメータを複数算出する平面算出手段と、この平面算出
工程から得られた複数の平面パラメータを投票空間に投
票してこの投票結果に基づき平面を決定する平面決定手
段とを有することを特徴とする。

【００３４】本発明に係るロボット装置は、供給された
入力情報に基づいて動作を行う自律型のロボット装置で
あって、３次元データ群から３点以上の３次元データを
サンプリングし、上記３次元データによって決まる１つ
の平面を示す平面パラメータを複数算出する平面算出手
段と、この平面算出工程から得られた複数の平面パラメ
ータを投票空間に投票してこの投票結果に基づき平面を
決定する平面決定手段とを有することを特徴とする。

【００３５】本発明に係るプログラムは、所定の動作を
コンピュータに実行させるためのプログラムであって、
３次元データ群から３点以上の３次元データをサンプリ
ングし、上記３次元データによって決まる１つの平面を
示す平面パラメータを複数算出する平面算出工程と、こ
の平面算出工程から得られた複数の平面パラメータを投
票空間に投票してこの投票結果に基づき平面を決定する
平面決定工程とを有することを特徴とする。

【００３６】本発明に係る記録媒体は、所定の動作をコ
ンピュータに実行させるためのプログラムを記録したコ
ンピュータ読み取り可能な記録媒体であって、３次元デ
ータ群から３点以上の３次元データをサンプリングし、
上記３次元データによって決まる１つの平面を示す平面
パラメータを複数算出する平面算出工程と、この平面算
出工程から得られた複数の平面パラメータを投票空間に
投票してこの投票結果に基づき平面を決定する平面決定
工程とを有することを特徴とする。

【００３７】

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

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

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

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

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

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

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

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

【００４５】

【数４】

【００４６】

【数５】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【００６１】

【数６】

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

【００６３】

【数７】

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

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

【００６６】

【数８】

【００６７】

【数９】

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

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

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

【００７１】

【数１０】

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

【００７３】

【数１１】

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

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

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

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

【００７８】

【数１２】

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

【００８０】

【数１３】

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

【００８２】

【数１４】

【００８３】

【数１５】

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

【００８５】

【数１６】

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

【００８７】

【数１７】

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

【００８９】

【数１８】

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

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

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

【００９３】

【数１９】

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

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

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

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

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

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

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

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

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

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

【０１０４】

【数２０】

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

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

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

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

【０１０９】

【数２１】

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

【０１１１】

【数２２】

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

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

【０１１４】

【数２３】

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

【０１１６】

【数２４】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【０１４７】

【数２５】

【０１４８】

【数２６】

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

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

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

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

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

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

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

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

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

【０１５８】

【数２７】

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

【０１６０】

【数２８】

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

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

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

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

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

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

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

【０１６８】

【数２９】

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

【０１７０】

【数３０】

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

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

【０１７３】

【数３１】

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

【０１７５】

【数３２】

【０１７６】ここで、▽Ｆ_ｌ，▽Ｆ_ａは、下記とする。

【０１７７】

【数３３】

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

【０１７９】

【数３４】

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

【０１８１】

【数３５】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【０２５９】

【数３６】

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

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

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

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

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

【０２６５】

【数３７】

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

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

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

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

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

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

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

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

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

【０２７５】

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

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

【図面の簡単な説明】

【図１】本発明の実施の形態におけるロボット装置の概
略を示すブロック図である。

【図２】本発明の実施の形態におけるロボット装置のソ
フトウェアの構成を示す模式図である。

【図３】本発明の実施の形態における平面抽出方法を示
すフローチャートである。

【図４】図３に示すステップＳ１の工程を説明するフロ
ーチャートである。

【図５】図３に示すステップＳ２及びＳ３の工程を説明
するフローチャートである。

【図６】本発明の実施の形態における平面パラメータを
示す模式図である。

【図７】３次元パラメータ投票空間を示す模式図であ
る。

【図８】本発明の実施の形態における３次元パラメータ
投票空間を示す模式図である。

【図９】図３に示すステップＳ４の工程を説明するフロ
ーチャートである。

【図１０】同じく、図３に示すステップＳ４の工程を示
すフローチャートである。

【図１１】図９に示すステップＳ４２及びＳ４３の工程
を示すフローチャートである。

【図１２】本発明の実施の形態におけるロボット装置の
ソフトウェアの動作を示すフローチャートである。

【図１３】同ソフトウェアに入力されるデータの流れを
示す模式図である。

【図１４】平面抽出部ＰＬＥＸに入力される視差画像の
生成を説明するための図である。

【図１５】平面抽出部ＰＬＥＸが障害物を認識する処理
手順を示すフローチャートである。

【図１６】平面検出部ＰＬＥＸが検出する平面パラメー
タを示す図である。

【図１７】カメラ座標系から足底接地平面座標系への変
換処理を説明するための図である。

【図１８】平面抽出部ＰＬＥＸにて抽出された平面上の
点を示す図である。

【図１９】ロボットビューから床面を抽出し、更に座標
変換して２次元（床平面）乗に障害物を表現したことを
示す図である。

【図２０】ランドマークセンサＣＬＳにおけるカラーラ
ンドマークを示す模式図である。

【図２１】ランドマークセンサＣＬＳの構成を模式的に
示すブロック図である。

【図２２】マルコフ・ローカリゼーション部により求め
られた各グリッド上での自己位置確率密度分布を示す模
式図である。

【図２３】ランドマークセンサＣＬＳ３４０の動作を示
すフローチャートである。

【図２４】障害物情報により生成された障害物地図上
に、現在位置から目的位置までの経路を示す模式図であ
る。

【図２５】経路計画アルゴリズムを示すフローチャート
である。

【図２６】本発明の実施の形態におけるロボット装置の
外観構成を示す斜視図である。

【図２７】同ロボット装置の自由度構成モデルを模式的
に示す図である。

【図２８】同ロボット装置の回路構成を示すブロック図
である。

【図２９】同ロボット装置のソフトウェア構成を示すブ
ロック図である。

【図３０】同ロボット装置のソフトウェア構成における
ミドル・ウェア・レイヤの構成を示すブロック図であ
る。

【図３１】同ロボット装置のソフトウェア構成における
アプリケーション・レイヤの構成を示すブロック図であ
る。

【図３２】アプリケーション・レイヤの行動モデルライ
ブラリの構成を示すブロック図である。

【図３３】同ロボット装置の行動決定のための情報とな
る有限確率オートマトンを説明する図である。

【図３４】有限確率オートマトンの各ノードに用意され
た状態遷移表を示す図である。

【符号の説明】

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

───────────────────────────────────────────────────── フロントページの続き (72)発明者 大橋 武史 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 (72)発明者 福地 正樹 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 (72)発明者 大久保 厚志 東京都品川区北品川６丁目７番35号 ソニ ー株式会社内 Ｆターム(参考） 3C007 AS36 CS08 KS12 KT03 KT11 KT15 MT04 MT08 WA03 WA13 WB21 5B057 AA05 BA02 CA01 CA13 CA16 DA08 DB03 DC02 DC13 5L096 AA02 AA09 BA05 CA05 FA24

Claims (28)

【特許請求の範囲】
1. 【請求項１】 ３次元データ群から３点以上の３次元デ
   ータをサンプリングし、上記３次元データによって決ま
   る１つの平面を示す平面パラメータを複数算出する平面
   算出工程と、 この平面算出工程から得られた複数の平面パラメータを
   投票空間に投票してこの投票結果に基づき平面を決定す
   る平面決定工程とを有することを特徴とする平面抽出方
   法。
2. 【請求項２】 上記平面算出工程は、上記平面の法線ベ
   クトルの向きを（θ，φ）、原点からの距離をｄとした
   とき、上記平面パラメータ（θ，φ，ｄ）を算出する工
   程であって、 上記投票空間は、下記数式（１−１）又は数式（１−
   ２）で表されるものであることを特徴とする請求項１記
   載の平面抽出方法。 【数１】
3. 【請求項３】 上記３次元データ群は、入力画像に基づ
   き３次元データ群に変換されるものであって、各上記３
   次元パラメータは上記入力画像に基づき算出される信頼
   度パラメータを個別に有し、この信頼度パラメータに基
   づいて上記３次元データ群の３次元データを選別するデ
   ータ選別工程を有することを特徴とする請求項１記載の
   平面抽出方法。
4. 【請求項４】 上記サンプリングデータは、３点をラン
   ダムにサンプリングしたものであって、上記ランダムサ
   ンプリングを複数回繰り返すことを特徴とする請求項１
   記載の平面抽出方法。
5. 【請求項５】 上記サンプリングデータは、ランダムに
   サンプリングされた１つの基準点と、この基準点から所
   定の距離内でランダムにサンプリングされた他の２点と
   からなることを特徴とする請求項１記載の平面抽出方
   法。
6. 【請求項６】 上記サンプリングデータは、上記３次元
   データ群を有する３次元空間を所定の領域に分割し、こ
   の分割された領域に含まれる３次元データとすることを
   特徴とする請求項１記載の平面抽出方法。
7. 【請求項７】 上記平面算出工程は、３点の上記３次元
   データから直接解法によって上記平面パラメータを算出
   する工程を有することを特徴とする請求項１記載の平面
   抽出方法。
8. 【請求項８】 上記平面算出工程は、３点以上の上記３
   次元データから共分散行列を固有値展開して最小固有値
   に対応するベクトルを抽出する主成分分析により上記平
   面パラメータを算出する工程を有することを特徴とする
   請求項１記載の平面抽出方法。
9. 【請求項９】 上記平面算出工程は、３点以上の上記３
   次元データから上記平面が原点を通らない任意の平面で
   あると仮定し最小二乗法を使用して上記平面パラメータ
   を算出する工程を有することを特徴とする請求項１記載
   の平面抽出方法。
10. 【請求項１０】 上記平面決定工程は、上記投票を繰り
    返し、上記投票の総数が所定の閾値を超えた場合、又は
    上記投票数に基づいて決定される投票値が最も高い位置
    における投票値の合計が全ての投票値の合計である総投
    票値の所定の割合に達した場合に投票をうち切る工程を
    有することを特徴とする請求項１記載の平面抽出方法。
11. 【請求項１１】 上記平面決定工程は、上記３次元デー
    タの信頼度及び／又は平面算出方法により異なる重みを
    定めて投票する重付投票工程を有することを特徴とする
    請求項１記載の平面抽出方法。
12. 【請求項１２】 上記平面決定工程は、上記投票空間の
    投票のピーク近傍における投票数による重み付き平均を
    行う重付平均工程を有することを特徴とする請求項１記
    載の平面抽出方法。
13. 【請求項１３】 上記平面決定工程により抽出された平
    面に含まれる３次元データが２次元となる座標変換行列
    を求め、上記３次元データを該座標変換行列により写像
    して出力する写像工程を有する請求項１記載の平面抽出
    方法。
14. 【請求項１４】 上記平面決定工程は、上記投票結果に
    より推定された平面パラメータを初期パラメータとして
    イタレーションにより該初期パラメータから最適パラメ
    ータを算出する最適パラメータ算出工程を有する請求項
    １記載の平面抽出方法。
15. 【請求項１５】 上記平面決定工程は、上記投票におけ
    る投票ピークの鋭さ、上記イタレーションに使用したデ
    ータポイント数、及び上記イタレーションにより算出さ
    れる誤差残差からなる群から選択された１以上を信頼度
    のパラメータとして使用し、決定される平面の信頼度を
    算出する信頼度算出工程を有することを特徴とする請求
    項１４記載の平面抽出方法。
16. 【請求項１６】 上記パラメータ算出工程は、上記初期
    パラメータを推定する平面算出工程における上記３次元
    データより、解像度が高い３次元データを使用してイタ
    レーションを行うことを特徴とする請求項１５記載の平
    面抽出方法。
17. 【請求項１７】 ３次元データ群から３点以上の３次元
    データをサンプリングし、上記３次元データによって決
    まる１つの平面を示す平面パラメータを複数算出する平
    面算出手段と、 この平面算出工程から得られた複数の平面パラメータを
    投票空間に投票してこの投票結果に基づき平面を決定す
    る平面決定手段とを有することを特徴とする平面抽出装
    置。
18. 【請求項１８】 上記平面算出手段は、上記平面の法線
    ベクトルの向きを（θ，φ）、原点からの距離をｄとし
    たとき、上記平面パラメータ（θ，φ，ｄ）を算出する
    工程であって、 上記投票空間は、下記数式（１−１）又は数式（１−
    ２）で表されるものであることを特徴とする請求項１７
    記載の平面抽出装置。 【数２】
19. 【請求項１９】 上記３次元データ群は、入力画像に基
    づき３次元データ群に変換されるものであって、各上記
    ３次元パラメータは上記入力画像に基づき算出される信
    頼度パラメータを個別に有し、この信頼度パラメータに
    基づいて上記３次元データ群の３次元データを選別する
    データ選別手段を有することを特徴とする請求項１７記
    載の平面抽出装置。
20. 【請求項２０】 上記平面算出手段は、上記３次元デー
    タから３点をサンプリングし直接解法によって上記平面
    パラメータを算出する第１のパラメータ算出手段、上記
    ３次元データから３点以上をサンプリングし共分散行列
    を固有値展開して最小固有値に対応するベクトルを抽出
    する主成分分析により上記平面パラメータを算出する第
    ２のパラメータ手段、及び上記３次元データから３点以
    上をサンプリングし上記平面が原点を通らない任意の平
    面であると仮定し最小二乗法を使用して上記平面パラメ
    ータを算出する第３のパラメータ算出手段からなる群か
    ら選択された１以上の算出手段を有することを特徴とす
    る請求項１７記載の平面抽出装置。
21. 【請求項２１】 上記平面決定手段は、上記投票を繰り
    返し、上記投票の総数が所定の閾値を超えた場合、又は
    上記投票数に基づいて決定される投票値が最も高い位置
    における投票値の合計が全ての投票値の合計である総投
    票値の所定の割合に達した場合に投票をうち切る投票中
    止手段を有することを特徴とする請求項１７記載の平面
    抽出装置。
22. 【請求項２２】 上記平面決定手段は、上記３次元デー
    タの信頼度及び／又は平面算出方法により異なる重みを
    定めて投票する重付投票工程を有することを特徴とする
    請求項１７記載の平面抽出装置。
23. 【請求項２３】 上記平面決定手段は、上記投票空間の
    投票のピーク近傍における投票数による重み付き平均を
    行う重付平均手段を有することを特徴とする請求項１７
    記載の平面抽出装置。
24. 【請求項２４】 上記平面決定手段は、上記投票結果に
    より推定された平面パラメータを初期パラメータとして
    イタレーションにより該初期パラメータから最適パラメ
    ータを算出する最適パラメータ算出手段を有する請求項
    １７記載の平面抽出装置。
25. 【請求項２５】 上記平面決定手段は、上記投票におけ
    る投票ピークの鋭さ、上記イタレーションに使用したデ
    ータポイント数、及び上記イタレーションにより算出さ
    れる誤差残差からなる群から選択された１以上を信頼度
    のパラメータとして使用し、決定される平面の信頼度を
    算出する信頼度算出手段を有することを特徴とする請求
    項２４記載の平面抽出装置。
26. 【請求項２６】 供給された入力情報に基づいて動作を
    行う自律型のロボット装置であって、 ３次元データ群から３点以上の３次元データをサンプリ
    ングし、上記３次元データによって決まる１つの平面を
    示す平面パラメータを複数算出する平面算出手段と、 この平面算出工程から得られた複数の平面パラメータを
    投票空間に投票してこの投票結果に基づき平面を決定す
    る平面決定手段とを有することを特徴とするロボット装
    置。
27. 【請求項２７】 所定の動作をコンピュータに実行させ
    るためのプログラムであって、 ３次元データ群から３点以上の３次元データをサンプリ
    ングし、上記３次元データによって決まる１つの平面を
    示す平面パラメータを複数算出する平面算出工程と、 この平面算出工程から得られた複数の平面パラメータを
    投票空間に投票してこの投票結果に基づき平面を決定す
    る平面決定工程とを有することを特徴とするプログラ
    ム。
28. 【請求項２８】 所定の動作をコンピュータに実行させ
    るためのプログラムを記録したコンピュータ読み取り可
    能な記録媒体であって、 ３次元データ群から３点以上の３次元データをサンプリ
    ングし、上記３次元データによって決まる１つの平面を
    示す平面パラメータを複数算出する平面算出工程と、 この平面算出工程から得られた複数の平面パラメータを
    投票空間に投票してこの投票結果に基づき平面を決定す
    る平面決定工程とを有することを特徴とするプログラム
    を記録した記録媒体。