以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、環境中を移動しながら未知の物体を自律的に学習する2足歩行タイプのロボット装置に適用したものである。このロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、人間が行う基本的な動作を表出できるエンターテインメントロボットでもある。
(1)ロボット装置の構成
図1に示すように、本実施の形態におけるロボット装置1は、体幹部ユニット2の所定の位置に頭部ユニット3が連結されると共に、左右2つの腕部ユニット4R/Lと、左右2つの脚部ユニット5R/Lが連結されて構成されている(但し、R及びLの各々は、右及び左の各々を示す接尾辞である。以下において同じ。)。
このロボット装置1が具備する関節自由度構成を図2に模式的に示す。頭部ユニット3を支持する首関節は、首関節ヨー軸101と、首関節ピッチ軸102と、首関節ロール軸103という3自由度を有している。
また、上肢を構成する各々の腕部ユニット4R/Lは、肩関節ピッチ軸107と、肩関節ロール軸108と、上腕ヨー軸109と、肘関節ピッチ軸110と、前腕ヨー軸111と、手首関節ピッチ軸112と、手首関節ロール軸113と、手部114とで構成される。手部114は、実際には、複数本の指を含む多関節・多自由度構造体である。但し、手部114の動作は、ロボット装置1の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書ではゼロ自由度と仮定する。したがって、各腕部は7自由度を有するとする。
また、体幹部ユニット2は、体幹ピッチ軸104と、体幹ロール軸105と、体幹ヨー軸106という3自由度を有する。
また、下肢を構成する各々の脚部ユニット5R/Lは、股関節ヨー軸115と、股関節ピッチ軸116と、股関節ロール軸117と、膝関節ピッチ軸118と、足首関節ピッチ軸119と、足首関節ロール軸120と、足部121とで構成される。本明細書中では、股関節ピッチ軸116と股関節ロール軸117の交点は、ロボット装置1の股関節位置を定義する。人体の足部121は、実際には多関節・多自由度の足底を含んだ構造体であるが、ロボット装置1の足底は、ゼロ自由度とする。したがって、各脚部は、6自由度で構成される。
以上を総括すれば、ロボット装置1全体としては、合計で3+7×2+3+6×2=32自由度を有することになる。但し、エンターテインメント向けのロボット装置1が必ずしも32自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
上述したようなロボット装置1がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、2足歩行という不安定構造体に対して姿勢制御を行うことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。
図3には、ロボット装置1の制御システム構成を模式的に示している。同図に示すように、ロボット装置1は、ヒトの四肢を表現した体幹部ユニット2,頭部ユニット3,腕部ユニット4R/L,脚部ユニット5R/Lと、各ユニット間の協調動作を実現するための適応制御を行う制御ユニット10とで構成される。
ロボット装置1全体の動作は、制御ユニット10によって統括的に制御される。制御ユニット10は、CPU(Central Processing Unit)や、DRAM(Dynamic Random Access Memory)、フラッシュROM(Read Only Memory)等の主要回路コンポーネント(図示せず)で構成される主制御部11と、電源回路やロボット装置1の各構成要素とのデータやコマンドの授受を行うインターフェイス(何れも図示せず)などを含んだ周辺回路12とで構成される。
本発明を実現するうえで、この制御ユニット10の設置場所は、特に限定されない。図3では体幹部ユニット2に搭載されているが、頭部ユニット3に搭載してもよい。あるいは、ロボット装置1外に制御ユニット10を配備して、ロボット装置1の機体とは有線又は無線で交信するようにしてもよい。
図2に示したロボット装置1内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。すなわち、頭部ユニット3には、首関節ヨー軸101、首関節ピッチ軸102、首関節ロール軸103の各々を表現する首関節ヨー軸アクチュエータA2、首関節ピッチ軸アクチュエータA3、首関節ロール軸アクチュエータA4が配設されている。
また、頭部ユニット3には、外部の状況を撮像するための一対のCCD(Charge Coupled Device)カメラが設けられているほか、前方に位置する物体までの距離を測定するための距離センサ、外部音を集音するためのマイクロホン、音声を出力するためのスピーカ、使用者からの「撫でる」や「叩く」といった物理的な働きかけにより受けた圧力を検出するためのタッチセンサ等が配設されている。
また、体幹部ユニット2には、体幹ピッチ軸104、体幹ロール軸105、体幹ヨー軸106の各々を表現する体幹ピッチ軸アクチュエータA5、体幹ロール軸アクチュエータA6、体幹ヨー軸アクチュエータA7が配設されている。また、体幹部ユニット2には、このロボット装置1の起動電源となるバッテリを備えている。このバッテリは、充放電可能な電池によって構成されている。
また、腕部ユニット4R/Lは、上腕ユニット41R/Lと、肘関節ユニット42R/Lと、前腕ユニット43R/Lに細分化されるが、肩関節ピッチ軸107、肩関節ロール軸108、上腕ヨー軸109、肘関節ピッチ軸110、前腕ヨー軸111、手首関節ピッチ軸112、手首関節ロール軸113の各々表現する肩関節ピッチ軸アクチュエータA8、肩関節ロール軸アクチュエータA9、上腕ヨー軸アクチュエータA10、肘関節ピッチ軸アクチュエータA11、肘関節ロール軸アクチュエータA12、手首関節ピッチ軸アクチュエータA13、手首関節ロール軸アクチュエータA14が配備されている。
また、脚部ユニット5R/Lは、大腿部ユニット51R/Lと、膝ユニット52R/Lと、脛部ユニット53R/Lに細分化されるが、股関節ヨー軸115、股関節ピッチ軸116、股関節ロール軸117、膝関節ピッチ軸118、足首関節ピッチ軸119、足首関節ロール軸120の各々を表現する股関節ヨー軸アクチュエータA16、股関節ピッチ軸アクチュエータA17、股関節ロール軸アクチュエータA18、膝関節ピッチ軸アクチュエータA19、足首関節ピッチ軸アクチュエータA20、足首関節ロール軸アクチュエータA21が配備されている。各関節に用いられるアクチュエータA2,A3・・・は、より好ましくは、ギア直結型で旦つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ACサーボ・アクチュエータで構成することができる。
体幹部ユニット2、頭部ユニット3、各腕部ユニット4R/L、各脚部ユニット5R/Lなどの各機構ユニット毎に、アクチュエータ駆動制御部の副制御部20,21,22R/L,23R/Lが配備されている。さらに、各脚部ユニット5R/Lの足底が接地したか否かを検出する接地確認センサ30R/Lを装着するとともに、体幹部ユニット2内には、姿勢を計測する姿勢センサ31を装備している。
接地確認センサ30R/Lは、例えば足底に設置された近接センサ又はマイクロ・スイッチなどで構成される。また、姿勢センサ31は、例えば、加速度センサとジャイロ・センサの組み合わせによって構成される。
接地確認センサ30R/Lの出力によって、歩行・走行などの動作期間中において、左右の各脚部ユニット5R/Lが現在立脚又は遊脚何れの状態であるかを判別することができる。また、姿勢センサ31の出力により、体幹部ユニット2の傾きや姿勢を検出することができる。
主制御部11は、各センサ30R/L,31の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部20,21,22R/L,23R/Lの各々に対して適応的な制御を行い、ロボット装置1の腕部ユニット4R/L、体幹部ユニット2、及び脚部ユニット5R/Lが協調して駆動する全身運動パターンを実現できる。
ロボット装置1の機体上での全身運動は、足部運動、ZMP(Zero Moment Point)軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容に従った動作を指示するコマンドを各副制御部20,21,22R/L,23R/Lに転送する。そして、各々の副制御部20,21,・・・等では、主制御部11からの受信コマンドを解釈して、各アクチュエータA2,A3・・・等に対して駆動制御信号を出力する。ここでいう「ZMP」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ZMP軌道」とは、例えばロボット装置1の歩行動作期間中にZMPが動く軌跡を意味する。なお、ZMPの概念並びにZMPを歩行ロボットの安定度判別規範に適用する点については、Miomir Vukobratovic 著“LEGGED LOCOMOTION ROBOTS”(加藤一郎外著『歩行ロボットと人工の足』(日刊工業新聞社))に記載されている。
以上のように、ロボット装置1は、各々の副制御部20,21,・・・等が、主制御部11からの受信コマンドを解釈して、各アクチュエータA2,A3・・・に対して駆動制御信号を出力し、各ユニットの駆動を制御している。これにより、ロボット装置1は、目標の姿勢に安定して遷移し、安定した姿勢で歩行できる。
また、ロボット装置1における制御ユニット10では、上述したような姿勢制御のほかに、加速度センサ、タッチセンサ、接地確認センサ30R/L等の各種センサ、及びCCDカメラからの画像データ、マイクロホンからの音声データ等を統括して処理している。制御ユニット10では、図示しないが加速度センサ、ジャイロ・センサ、タッチセンサ、距離センサ、マイクロホン、スピーカなどの各種センサ、各アクチュエータ、CCDカメラ及びバッテリが各々対応するハブを介して主制御部11と接続されている。
主制御部11は、上述の各センサから供給されるセンサデータや画像データ及び音声データを順次取り込み、これらをそれぞれ内部インターフェイスを介してDRAM内の所定位置に順次格納する。また、主制御部11は、バッテリから供給されるバッテリ残量を表すバッテリ残量データを順次取り込み、これをDRAM内の所定位置に格納する。DRAMに格納された各センサデータ、画像データ、音声データ及びバッテリ残量データは、主制御部11がこのロボット装置1の動作制御を行う際に利用される。
主制御部11は、ロボット装置1の電源が投入された初期時、制御プログラムを読み出し、これをDRAMに格納する。また、主制御部11は、上述のように主制御部11よりDRAMに順次格納される各センサデータ、画像データ、音声データ及びバッテリ残量データに基づいて自己及び周囲の状況や、使用者からの指示及び働きかけの有無などを判断する。
さらに、主制御部11は、この判断結果及びDRAMに格納した制御プログラムに基づいて自己の状況に応じて行動を決定するとともに、当該決定結果に基づいて必要なアクチュエータA2,A3・・・を駆動させることにより、ロボット装置1に、いわゆる「身振り」、「手振り」といった行動をとらせる。
このようにしてロボット装置1は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
(1−2)ロボット装置のソフトウェア構成
上述したロボット装置1のソフトウェア構成を図4の模式図に示す。図4において、丸で表されているのがオブジェクト又はプロセスと呼ばれるエンティティである。このオブジェクト同士が非同期に通信し合うことで、システム全体が動作する。各オブジェクトは、メッセージ通信と共有メモリを使用したオブジェクト間通信とにより、データの受け渡し及びInvokeを行っている。
図4に示すように、ソフトウェアは、キネマティックオドメトリ(Kinematics Odometry)KINE200、平面抽出部(Plane Extractor)PLEX210、グリッド占有率算出部(Occupancy Grid)OG220、ランドマークセンサ部(Landmark Sensor)CLS230、絶対座標算出部(Localization)LZ240及び経路計画決定部(Situated Behavior Layer)SBL250から構成され、各オブジェクト単位にて処理がなされる。このソフトウェアの動作について、図5のフローチャートを用いて詳細に説明する。
図4に示すキネマティックオドメトリKINE200には、左右のCCDカメラで撮像した画像データD1及びロボット装置1の関節角等のセンサデータD2が入力される。ステップS1において、キネマティックオドメトリKINE200は、これらの画像データD1及びセンサデータD2を受け取り、メモリに格納されていたそれまでの画像データ及びセンサデータを更新する。
次にステップS2において、キネマティックオドメトリKINE200は、画像データD1とセンサデータD2との時間的な対応を取る。具体的には、画像データD1の画像が撮像された時間におけるセンサデータD2の関節角を割り出す。
続いてステップS3において、キネマティックオドメトリKINE200は、この関節角のデータを使用してロボット装置1を中心としたロボット中心座標系を頭部ユニット3に設けられたCCDカメラを中心としたカメラ座標系に変換する。この際、キネマティックオドメトリKINE200は、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列D3とこれに対応する画像データとを画像認識を行うオブジェクトに供給する。具体的には、同次変換行列D3及び左右の画像データから得られた視差画像データD4を平面抽出部PLEX210に供給し、同次変換行列D3及び画像データD5をランドマークセンサ部CLS230に供給する。また、キネマティックオドメトリKINE200は、センサデータD2から得られる歩行パラメータと接地確認センサ30R/Lを使用した歩数のカウント値とからロボット装置1の移動量を求め、ロボット装置1のロボット中心座標系における移動量(以下、「オドメトリ」ともいう。)を算出する。キネマティックオドメトリKINE200は、このオドメトリD6をグリッド占有率算出部OG220及び絶対座標算出部LZ240に供給する。
続いてステップS4,S5において、平面抽出部PLEX210及びグリッド占有率算出部OG220は、障害物のグリッド占有率(環境地図)を算出・更新し、ランドマークセンサ部CLS210及び絶対座標算出部LZ240は、絶対座標を算出・更新する。
ここで、このステップS4,S5での平面抽出部PLEX210及びグリッド占有率算出部OG220における処理と、ランドマークセンサ部CLS230及び絶対座標算出部LZ240における処理とは、それぞれ図6(A),(B)に示すように、同時並行的に行われるものである。
すなわち、障害物グリッドを算出・更新する場合、平面抽出部PLEX210は、先ず図6(A)のステップS11において、キネマティックオドメトリKINE200にて算出されたキネマティックスデータである同次変換行列D3と、これに対応する視差画像データD4とが入力されると、それまでメモリに格納されていたこれらのデータを更新する。
次にステップS12において、平面抽出部PLEX210は、CCDカメラのキャリブレーションパラメータ等を使用して視差画像データD4から3次元の位置データ(レンジデータ)を算出する(例えば特開2000−350239号公報を参照)。平面抽出部PLEX210は、このレンジデータからハフ変換等を使用して、例えば壁やテーブル等の平面を除く平面を平面として抽出する。そしてステップS13において、平面抽出部PLEX210は、同次変換行列D3からロボット装置1の足底が接地している平面との対応を取って床面を選択し、床面上にない点、例えば所定の閾値よりも高い位置にあるもの等を障害物として検出し、この障害物情報D7をグリッド占有率算出部220に供給する。
続いてステップS14において、グリッド占有率算出部OG220は、キネマティックオドメトリKINE200にて算出されたオドメトリと、平面抽出部PLEX210にて算出された障害物の観測情報(障害物情報)とが入力されると、メモリに格納されていたそれまでのデータを更新する。そしてステップS15において、グリッド占有率算出部OG220は、床面上に障害物があるか否かの確率を保持する障害物のグリッド占有率を確率的手法により更新する。
このグリッド占有率算出部OG220は、ロボット装置1を中心とした例えば周囲4mの障害物情報(障害物のグリッド占有率)を示す環境地図と、ロボット装置1が向く方角を示す姿勢情報とを保持している。ここで、環境地図は、例えば図7に示すように、ロボット装置1を中心とした周囲の環境が複数のグリッドに分割されたものであり、各グリッドは、障害物が存在するグリッド(図中黒色で表されるグリッド)と、障害物が存在しないグリッド(図中白色で表されるグリッド)と、未観測のグリッド(図中斜線で表されるグリッド)とに分類される。グリッド占有率算出部OG220は、このような環境地図を更新し、更新した認識結果(障害物情報D9)を経路計画決定部SBL250に供給することにより、上位レイヤ、すなわち経路計画決定部SBL250にて障害物を回避する計画を作成することができる(例えば「佐部、大橋、河本、グットマン、福地、芦ヶ原、『二足歩行ロボットによるステレオ画像を用いた障害物回避と歩行計画』、第8回画像センシングシンポジウム講演論文集 p.237−242」を参照)。
一方、絶対座標を算出・更新する場合、ランドマークセンサ部CLS230は、先ずステップS21において、キネマティックオドメトリKINE200からキネマティックスデータである同次変換行列D3及び画像データD5が入力されると、予めメモリ内に格納されていたこれらのデータを更新する。そして、画像データD5の画像処理を行って、予め認識しているカラーランドマークを検出する。このカラーランドマークの画像データD5上での位置及び大きさをカメラ座標系での位置に変換する。さらにステップS22において、ランドマークセンサ部CLS230は、同次変換行列D3を使用してカラーランドマークのカメラ座標系での位置をロボット中心座標系における位置に変換し、ロボット中心座標系におけるカラーランドマーク位置の情報(カラーランドマーク相対位置情報)D8を絶対座標算出部LZ240に供給する。
すなわち、ランドマークセンサ部CLS230は、例えば、グリーン、ピンク、又はブルー等のカラーを有する人工的なカラーランドマークを含む環境において、ロボット装置1のセンサ情報とロボット装置1が行った動作情報とによって、ロボット装置1の自己位置(位置及び姿勢)を同定する。
続いてステップS23において、絶対座標算出部LZ240は、キネマティックオドメトリKINE200からのオドメトリD6とランドマークセンサ部CLS230からのカラーランドマーク相対位置情報D8とが入力されると、予めメモリ内に格納されていたこれらのデータを更新する。
続いてステップS24において、絶対座標算出部LZ240は、予め認識しているカラーランドマークの絶対座標(カメラ座標系での位置)、カラーランドマーク相対位置情報D8及びオドメトリD6を使用し、確率的手法によりロボット装置1の絶対座標(カメラ座標系での位置)を算出する。そして、この絶対座標位置D10を経路計画決定部SBL250に供給する。
図5に戻って、経路計画決定部SBL250は、グリッド占有率算出部OG220から障害物情報D9が入力され、絶対座標算出部LZ240から絶対座標位置D10が入力されると、予めメモリに格納されていたこれらのデータを更新する。そして、ステップS6において、経路計画決定部SBL250は、グリッド占有率算出部OG220からの障害物情報D9により、ロボット装置1の周囲に存在する障害物に関する認識結果を取得し、絶対座標算出部LZ240から現在のロボット装置1の絶対座標を取得することにより、カメラ座標系又はロボット中心座標系で与えられた目標地点に対して障害物に衝突せずに歩行可能な経路を生成し、経路に応じて経路を遂行する動作コマンドを発行する。すなわち、入力データから状況に応じてロボット装置1が取るべき行動を決定し、その行動列を出力する。
以上のようなプログラムにより、ロボット装置1は、周囲に存在する障害物に衝突することなく、目標地点までの経路を計画して歩行することができる。
(2)物体認識システム
次に、上述した自律的に行動可能なロボット装置1に搭載される物体認識システムについて説明する。この物体認識システムの動作には、未知の物体の視覚的特徴を学習する物体学習フェーズと、この学習結果に基づいて学習済みの物体を認識する物体認識フェーズとがある。
ここで、ロボット装置1は、上述した通り、ロボット装置1を中心とした周囲の障害物情報(障害物のグリッド占有率)を示す環境地図を作成し、この環境地図中の障害物情報に基づいて障害物に衝突しないような経路を計画し、目標地点まで自律的に歩行するものであり、環境に適用、作用するために認識する必要がある物体として、環境中の障害物を学習する。
このような自律型のロボット装置1が行動するような実環境において、ロボット装置1自身が自律的に移動して環境中の物体を学習・認識する場合には、視点や照明環境の変化に頑強な物体認識手法が必要とされる。そこで、本実施の形態では、このような物体認識手法の一例として、本件出願人が先に提案した特願2003−124225の明細書及び図面に記載された手法を用いることとし、これから学習しようとする物体の画像(以下、適宜「モデル画像」という。)と入力画像(以下、適宜「オブジェクト画像」という。)とから特徴点及びその特徴量を抽出し、類似する特徴量を有する特徴点のペア(候補対応特徴点ペア)を生成して、生成された候補対応特徴点ペア群を用いてオブジェクト画像中のモデル、すなわち入力画像中の既学習物体を認識する。
なお、実際には、物体学習フェーズにおいて、複数のモデル画像の各特徴点における特徴量を予め学習しておき、物体認識フェーズにおいて、入力されたオブジェクト画像の特徴点における特徴量と複数のモデル画像についての特徴量とを比較し、オブジェクト画像中のモデルを検出するが、以下では、先ず、物体学習フェーズにおけるモデル画像からの特徴量抽出と物体認識フェーズにおけるオブジェクト画像からの特徴量抽出とを並行して行うものとして説明する。
本実施の形態における物体認識システムの概略構成を図8に示す。図8に示す物体認識システム300において、特徴点抽出部310a,310bは、モデル画像及びオブジェクト画像からそれぞれモデル特徴点及びオブジェクト特徴点を抽出し、特徴量保持部320a,320bは、抽出された各特徴点について後述する特徴量を抽出し、特徴点の位置情報と共に保持する。そして特徴量比較部330は、各モデル特徴点の特徴量と各オブジェクト特徴点の特徴量とを比較して類似度又は相違度を計算し、この類似尺度を用いて特徴量が類似する、すなわち対応する可能性の高いモデル特徴点及びオブジェクト特徴点のペア(候補対応特徴点ペア)を生成する。
モデル姿勢推定部340は、生成された候補対応特徴点ペア群を用いてオブジェクト画像上のモデルの有無を検出し、「モデル有」の判定結果の場合には、「検出されるモデルはオブジェクト画像上へのアフィン変換による画像変形が施されている」という拘束条件の下、候補対応特徴点ペア群からランダムに選択した3ペアにより決定されるアフィン変換パラメータをパラメータ空間に投射する操作を繰り返し、パラメータ空間上で形成されたクラスタのうち最多メンバ数を持つクラスタの各メンバを真の特徴点ペア(インライヤ)とし、このインライヤを用いた最小自乗推定によりアフィン変換パラメータを求める。そして、モデル姿勢推定部340は、このアフィン変換パラメータで決定されるモデル姿勢をモデル認識結果として出力する。
以下、上記物体認識システム300の各ブロックについて詳細に説明する。なお、以下では、画像の水平方向をX軸、垂直方向をY軸として説明する。
特徴点抽出部310a,310bは、特徴点抽出対象画像(モデル画像又はオブジェクト画像)に対し、先ず平滑化フィルタリング、例えば以下の式(1)で示す2次元ガウス関数による畳み込み積(ガウスフィルタリング)と、双2次線形補間リサンプリングによる画像縮小とを繰り返し交互に適用することで、画像の多重解像度ピラミッド構造を構築する。ここで、リサンプリングファクタとしては、式(1)のガウスフィルタで用いたσを用いる。
すなわち、図9に示すように、例えば入力画像Iにσ=√2であるガウスフィルタg(x、y)を施すことにより第1レベル(最高解像度)の画像I1を生成し、さらにガウスフィルタを施すことにより画像g*I1を生成する。そして、この画像g*I1をリサンプリングしてガウスフィルタを施すことにより第2レベルの画像I2,g*I2を生成し、同様にして画像g*I2から第3レベルの画像I3,g*I3を生成する。
続いて、特徴点抽出部310a,310bは、各レベル(各解像度)の画像に対して、DoG(Difference of Gaussian)フィルタを適用する。このDoGフィルタは、画像の輪郭強調のために用いられる2次微分フィルタの一種であり、人間の視覚系で網膜からの情報が外側膝状体で中継されるまでに行われている処理の近似モデルとして、LoG(Laplacian of Gaussian)フィルタと共によく用いられるものである。DoGフィルタの出力は、2つのガウスフィルタ出力画像の差分を取ることにより容易に得られる。すなわち、図9に示すように、第1レベルの画像について画像DI1(=I1−g*I1)が得られ、第2レベル,第3レベルの画像について画像DI2(=I2−g*I2),画像DI3(=I3−g*I3)が得られる。
そして、特徴点抽出部310a,310bは、各レベルにおけるDoGフィルタ出力画像DI1,DI2,DI3・・・の局所点(局所極大点及び局所極小点)のうち、所定の範囲内の解像度変化によって位置の変化がないような点を特徴点として検出する。これにより、画像の拡大縮小操作に対してロバストな特徴点間のマッチングが実現できる。
ここで、多重解像度ピラミッド構造の第Lレベル、すなわちσの(L−1)乗ファクタまでの解像度変化によって位置の変化がないような特徴点を検出する際の処理について図10のフローチャートを用いて説明する。
先ずステップS31において、第1レベル(最高解像度)におけるDoGフィルタ出力画像DI1の局所点(局所極大点及び局所極小点)を検出する。なお、局所近傍としては、例えば3×3直接近傍を用いることができる。
次にステップS32において、検出された各局所点について、その1つ上位レベル(解像度の1つ低い層)における対応点を解像度減少に伴う画像縮小を考慮して求め、この対応点が局所点か否かを判別する。局所点である場合(Yes)にはステップS33に進み、局所点でない場合(No)には探索を終了する。
続いてステップS33では、第Lレベルまで探索が成功したか否かを判別する。第Lレベルまで探索を行っていない場合(No)にはステップS32に戻ってさらに上位レベルの探索を行い、第Lレベルまで探索が成功した場合(Yes)には特徴点であるとして、ステップS34においてその位置情報を保持する。
例えば第3レベルまでの解像度変化によって位置の変化がないような特徴点を検出する場合、図11に示すように、第1レベルの画像DI1において検出された局所点FP1,FP2のうち、FP1は第3レベルの画像DI3まで対応点が存在するため特徴点とされ、FP2は第2レベルまでしか対応点が存在しないため特徴点でないとされる。
なお、この特徴点抽出部310a,310bでは、DoGフィルタの代わりにLoGフィルタを用いるようにしても構わない。また、文献「Harris C. and Stephens M.,“A combined corner and edge detector.”, in Proc. Alvey Vision Conf., pp.147-151, 1988」で物体のコーナー検出に用いられているcorner-ness 関数の出力値をDoGフィルタ出力の代わりに用いるようにしても構わない。
次に、特徴量保持部320a,320b(図8)は、特徴点抽出部310a,310bにて抽出された各特徴点について特徴量を抽出し、保持する。特徴量としては、多重解像度ピラミッド構造の各レベルの画像(Il,l=1,…,L)の画像情報から導出される特徴点の近傍領域各点の濃度勾配情報(勾配強度及び勾配方向)を用いる。点(x、y)における勾配強度Mx,y及び勾配方向Rx,yは以下の式(2),(3)により与えられる。
ここで、特徴量を算出する特徴点近傍領域としては、回転変化に対して構造の変わらないような、特徴点に関して対称なものを選択することが好ましい。これにより、回転変化に対するロバスト性を実現することができる。例えば、(i)特徴点から半径rピクセルの範囲内を特徴点近傍領域とする手法や、(ii)特徴点を中心に幅σの特徴点に関して対称な2次元ガウス重みを濃度勾配に掛ける手法を用いることができる。
特徴点から半径3.5ピクセルの範囲内を近傍領域とした場合における特徴点近傍領域の濃度勾配情報の例を図12(A)に示す。ここで、図12(A)において矢印の長さは勾配強度を表し、矢印の方向は勾配方向を表す。
また、特徴量保持部320a,320bは、特徴点近傍の勾配方向に関するヒストグラム(方向ヒストグラム)も特徴量として保持する。図12(A)の濃度勾配情報から得られる勾配方向ヒストグラムの例を図12(B)に示す。ここで、図12(B)における階級幅Δθは10deg であり、階級数Nは36(=360deg /10deg )である。
続いて、特徴量比較部330(図8)は、各モデル特徴点の特徴量と各オブジェクト特徴点の特徴量とを比較し、類似する特徴量を有するモデル特徴点及びオブジェクト特徴点のペア(候補対応特徴点ペア)を生成する。
この特徴量比較部330における処理の詳細について、図13のフローチャートを用いて説明する。先ず、ステップS41において、特徴量比較部330は、各モデル特徴点の方向ヒストグラムと各オブジェクト特徴点の方向ヒストグラムとを比較してヒストグラム間の距離(相違度)を計算すると共に、モデル−オブジェクト間の推定回転角度を求める。
ここで、階級幅Δθと階級数Nとが同一である2つの方向ヒストグラムH1={h1(n),n=1,…,N}及びH2={h2(n),n=1,…,N}を想定し、h1(n),h2(n)が階級nにおける度数を示すものとすると、ヒストグラムH1とヒストグラムH2との間の距離d(H1,H2)は、例えば以下の式(4)で与えられる。ここで、式(4)におけるrとしては、r=1,2,∞が一般的に用いられる。
この式(4)を用いて各モデル特徴点及び各オブジェクト特徴点の方向ヒストグラム間の相違度を計算するが、(i)モデル−オブジェクト間のスケール比がマッチング段階で未知であるため、モデル特徴点の各レベルとオブジェクト特徴点の各レベル間で方向ヒストグラム間のマッチングを行う必要がある。また、(ii)方向ヒストグラム間のマッチングに関しては、モデル−オブジェクト間の回転変換量を考慮する必要がある。
モデル特徴点mのレベルLVにおける方向ヒストグラムHm LV={hm LV(n),n=1,…,N}と、オブジェクト特徴点oのレベルlvにおける方向ヒストグラムHo lv={ho lv(n),n=1,…,N}との相違度を求める場合を考える。方向ヒストグラムは回転変換に対して巡回的に変化するため、Ho lvを巡回的に階級を1つずつシフトさせながら式(4)の計算を行い、その最小値をHm LVとHo lvとの間の相違度とする。このとき、相違度の最小値を与えた時のシフト量(ずらした階級数)からオブジェクト特徴点の回転角度を推定することができる。なお、この手法は方向ヒストグラム交差法として知られている。
Ho lvをk階級分シフトさせた方向ヒストグラムをHo lv(k)とすると、方向ヒストグラム交差法による方向ヒストグラム間相違度dissimilarity(Hm LV,Ho lv(k))は、以下の式(5)で与えられる。
また、最小のd(Hm LV,Ho lv(k))を与えるkをk’とすると、オブジェクト特徴点oの近傍領域における推定回転角度θ(m,LV,o,lv)は以下の式(6)で与えられる。
上述の(i)を考慮すると、モデル特徴点mとオブジェクト特徴点oとの方向ヒストグラム間相違度dissimilarity(Hm,Ho)は、以下の式(7)のようになる。
特徴量比較部330は、モデル特徴点mとオブジェクト特徴点oとの各ペア(m、n)に対し、最小の方向ヒストグラム間相違度dissimilarity(Hm,Ho)を与えるレベルLV,lv(以下、それぞれLVm *,lvo *と表記する。)と、その推定回転角度θ(m,LVm *,o,lvo *)とを、方向ヒストグラム間相違度dissimilarity(Hm,Ho)と共に保持する。
次にステップS42(図13)において、特徴量比較部330は、各モデル特徴点mに対し方向ヒストグラム間相違度の小さい順にK個のオブジェク特徴点om1,…,omKを選択し、候補対応特徴点ペアを組ませる。すなわち、各モデル特徴点mに対してK個の候補対応特徴点ペア(m、om1),…,(m、omk),…,(m、omK)が組まれる。また、各候補対応特徴点ペア(m、omk)には対応するレベルLVm *,lvomk *と推定回転角度θ(m,LVm *,o,lvomk *)との情報が保持される。
以上のようにして、全モデル特徴点に対し候補対応特徴点ペアを組ませることで得られたペア群が、候補対応特徴点ペア群となる。
このように、特徴量比較部330では、ヒストグラム度数に勾配強度を累積しておらず、単に勾配方向のみに注目しているため、明度変化に対してロバストな特徴量マッチングが可能になる。また、方向ヒストグラムの形状を考慮したより安定なマッチングを行うことができ、二次的に安定した特徴量(推定回転角度)を得ることもできる。
なお、上述のステップS42では、各モデル特徴点mに対してK個の候補対応特徴点ペアを選択するものとして説明したが、これに限定されるものではなく、方向ヒストグラム間相違度が閾値を下回るペア全てを選択するようにしても構わない。
ここで、上述の操作で生成された候補対応特徴点ペア群は、方向ヒストグラムが類似するものの濃度勾配の空間的特徴が異なっているような対応点ペアも含んでいる。そこで、続くステップS43(図13)では、濃度勾配ベクトル間類似度によるペアの選択、候補対応特徴点ペア群の更新を行う。
具体的には、モデル特徴点mの近傍のレベルLVm *における濃度勾配ベクトルをUmとし、このモデル特徴点mと対応点ペアを組むオブジェクト特徴点oの近傍のレベルlvomk *における濃度勾配ベクトルをUoとしたとき、UmとUoとの類似度が閾値を下回るペアを排除し、候補対応特徴点ペア群を更新する。
濃度勾配ベクトルUm,Uo間の類似度の計算手法を図14に示す。先ず、Umを空間的に4領域Ri(i=1,…,4)に分割し、各領域の平均濃度勾配ベクトルvi(i=1,…,4)を求める。このviをまとめた8次元ベクトルVでUmが表現される。一方、回転変換を考慮した濃度勾配情報のマッチングを行うために、Uoの勾配方向を先に求めた推定回転角度θ(m,LVm *,o,lvomk *)で補正し、Uo *を得る。この際、中間位置の値は、双2次線形補間により求める。先と同様にUo *を4領域Ri(i=1,…,4)に分割し、各領域の平均濃度勾配ベクトルwi(i=1,…,4)を求める。このwiをまとめた8次元ベクトルWでUoが表現される。このとき、UmとUoとの類似度similarity(Um、Uo)∈[0,1]は、平均濃度勾配ベクトルVとWとの類似度として解釈され、例えばコサイン相関値を用いて以下の式(8)により求められる。ここで、式(8)において(V・W)はVとWとの内積を表す。
特徴量比較部330は、各候補対応特徴点ペアに対して上記式(8)で得られる平均濃度勾配ベクトル間の類似度を求め、類似度が閾値δを下回るペアを候補対応特徴点ペア群から排除し、候補対応特徴点ペア群を更新する。
このように、特徴量比較部330では、部分領域の平均濃度勾配ベクトルを用いて特徴量の比較を行っているため、特徴点位置や推定回転角度の微妙なずれや、明度の変化による濃度勾配情報の変化に対してロバストなマッチングを実現することができ、計算量の削減にもなる。
以上の操作により、特徴点近傍の局所的な濃度勾配情報が類似する(モデル特徴点−オジェクト特徴点)ペア群を抽出することができるが、巨視的に見ると、このように得られたペア群は、対応特徴点間の空間的位置関係がモデルのオブジェクト画像上での姿勢(モデル姿勢)と矛盾するような「偽の特徴点ペア(アウトライヤ)」を含んでしまっている。
ここで、候補対応特徴点ペアが3組以上ある場合には、最小自乗推定により近似アフィン変換パラメータの推定が可能であり、推定モデル姿勢と空間的位置関係の矛盾する対応ペアを排除し、残ったペアで再びモデル姿勢推定を行うという操作を繰り返すことで、モデル姿勢を認識することができる。
しかしながら、候補対応特徴点ペア群中のアウトライヤが多い場合や真のアフィン変換パラメータから極端に逸脱したアウトライヤが存在する場合には、最小自乗推定による推定結果は一般的に満足のいくものではないことが知られている(Hartley R., Zisserman A.,“Multiple View Geometry in Computer Vision.”, Chapter 3, pp.69-116, Cambridge University Press, 2000)ため、本実施の形態におけるモデル姿勢推定部340(図8)では、アフィン変換拘束の下、候補対応特徴点ペア群の空間的位置関係から「真の特徴点ペア(インライヤ)」を抽出し、抽出されたインライヤを用いてモデル姿勢(平行移動量,回転,拡大縮小,ストレッチを決定するアフィン変換パラメータ)を推定する。
以下、このモデル姿勢推定部340における処理について説明する。上述したように、候補対応特徴点ペアが3組以上なければアフィン変換パラメータを決定できないため、候補対応特徴点ペアが2組以下の場合、モデル姿勢推定部340は、オブジェクト画像中にモデルが存在しない、又はモデル姿勢検出に失敗したとして、「認識不可」と出力し処理を終了する。一方、候補対応特徴点ペアが3組以上ある場合、モデル姿勢推定部340は、モデル姿勢を検出可能であるとし、アフィン変換パラメータの推定を行う。なお、モデル姿勢推定部340は、モデル画像及びオブジェクト画像の例えば第1レベル(最高解像度)における特徴点の空間的位置を元にモデル姿勢を推定するものとする。
ここで、モデル特徴点[x y]Tのオブジェクト特徴点[u v]Tへのアフィン変換は、以下の式(9)で与えられる。
この式(9)において、ai(i=1,…,4)は回転,拡大縮小,ストレッチを決定するパラメータを表し、[b1 b2]Tは平行移動パラメータを表す。決定すべきアフィン変換パラメータはa1,…,a4及びb1,b2の6つであるため、候補対応特徴点ペアが3組あればアフィン変換パラメータを決定することができる。
3組の候補対応特徴点ペアで構成されるペア群Pを([x1 y1]T,[u1 v1]T),([x2 y2]T,[u2 v2]T),([x3 y3]T,[u3 v3]T)とすると、ペア群Pとアフィン変換パラメータとの関係は、以下の式(10)に示す線形システムで表現することができる。
この式(10)をRx=sのように書き直すと、アフィン変換パラメータxの最小自乗解は以下の式(11)で与えられる。
さて、候補対応特徴点ペア群からアウトライヤが1つ以上混入するように繰り返しランダムにペア群Pを選択した場合、そのアフィン変換パラメータはパラメータ空間上に散らばって投射される。一方、インライヤのみから構成されるペア群Pを繰り返しランダムに選択した場合、そのアフィン変換パラメータは、何れもモデル姿勢の真のアフィン変換パラメータに極めて類似した、すなわちパラメータ空間上で距離の近いものとなる。したがって、候補対応特徴点ペア群からランダムにペア群Pを選択し、そのアフィン変換パラメータをパラメータ空間上に投射していく操作を繰り返すと、インライヤはパラメータ空間上で密度の高い(メンバ数の多い)クラスタを形成し、アウトライヤは散らばって出現することになる。このことから、パラメータ空間上でクラスタリングを行い、最多メンバ数を持つクラスタの要素がインライヤとなる。
モデル姿勢推定部340における処理の詳細を図15のフローチャートを用いて説明する。なお、このモデル姿勢推定部340におけるクラスタリング手法としては、NN(Nearest Neighbor)法を用いるものとする。この際、上述したb1,b2は、認識対象画像により様々な値を取り得るため、x空間でもクラスタリングにおいてクラスタリング閾値の選択が認識対象に依存してしまう。そこで、モデル姿勢推定部340では、「真のパラメータとa1,…,a4は類似するがb1,b2が異なるようなアフィン変換パラメータを与えるペア群Pは殆ど存在しない」という仮定の下、パラメータa1,…,a4(以下、Aと表記する。)で張られるパラメータ空間上のみでクラスタリングを行う。なお、上記仮定が成り立たない状況が生じたとしても、A空間とは独立にb1,b2で張られるパラメータ空間でクラスタリングを行いその結果を考慮することで、容易に問題を回避することができる。
先ず図15のステップS51において初期化を行う。具体的には、繰り返し数のカウント値cntをcnt=1とし、候補対応特徴点ペア群からランダムにペア群P1を選択し、アフィン変換パラメータA1を求める。また、クラスタ数NをN=1とし、アフィン変換パラメータ空間A上でA1を中心とするクラスタZ1を作る。このクラスタZ1のセントロイドC1をC1=A1とし、メンバ数nz1をnz1=1とする。カウンタをcnt=2にし、ステップS52に進む。
次にステップS52において、モデル姿勢推定部340は、候補対応特徴点ペア群からランダムにペア群Pcntを選択し、アフィン変換パラメータAcntを求める。
続いてステップS53において、モデル姿勢推定部340は、NN法を用いてアフィン変換パラメータ空間のクラスタリングを行う。具体的には、先ず以下の式(12)に従ってアフィン変換パラメータAcntと各クラスタZiのセントロイドCi(i=1,…,N)との距離d(Acnt、Ci)のうち、最小の距離dminを求める。
そして、所定の閾値τ(例えばτ=0.1)に対してdmin<τであればdminを与えるクラスタZiにAcntを属させ、Acntを含めた全メンバでクラスタZiのセントロイドCiを更新する。また、クラスタZiのメンバ数nziをnzi=nzi+1とする。一方、dmin≧τであればアフィン変換パラメータ空間A上でAcntをセントロイドCN+1とする新しいクラスタZN+1を作り、メンバ数nzN+1をnzN+1=1とし、クラスタ数NをN=N+1とする。
続いてステップS54では、繰り返し終了条件を満たすか否かが判別される。ここで、繰り返し終了条件としては、例えば最多メンバ数が所定の閾値(例えば15)を超え、且つ最多メンバ数と2番目に多いメンバ数との差が所定の閾値(例えば3)を超える場合、或いは繰り返し数カウンタのカウント値cntが所定の閾値(例えば5000回)を超える場合に終了するように設定することができる。ステップS54において、繰り返し終了条件を満たさない場合(No)には、ステップS55で繰り返し数のカウント値cntをcnt=cnt+1とした後、ステップS52に戻る。一方、繰り返し終了条件を満たす場合(Yes)にはステップS56に進む。
最後にステップS56において、モデル姿勢推定部340は、以上で得られたインライヤを用いて、最小自乗法によりモデル姿勢を決定するアフィン変換パラメータを推定する。
ここで、インライヤを([xIN1 yIN1]T,[uIN1 vIN1]T),([xIN2 yIN2]T,[uIN2 vIN2]T),…とすると、インライヤとアフィン変換パラメータとの関係は、以下の式(13)に示す線形システムで表現することができる。
この式(13)をRINxIN=sINのように書き直すと、アフィン変換パラメータxINの最小自乗解は以下の式(14)で与えられる。
そしてステップS56では、このアフィン変換パラメータxINで決定されるモデル姿勢をモデル認識結果として出力する。
なお、以上の説明では、閾値τが定数値であるものとしたが、ステップS52乃至ステップS55の繰り返し処理を行う際に、始めは比較的大きな閾値τを用いて大雑把なインライヤ抽出を行い、繰り返し回数が増える毎に次第に小さい閾値τを用いる、いわゆる「焼きなまし法」のような手法を適用してもよい。これにより、精度よくインライヤを抽出することができる。
また、以上の説明では、候補対応特徴点ペア群からランダムにペア群Pを選択し、そのアフィン変換パラメータをパラメータ空間上に投射していく操作を繰り返し、パラメータ空間上で最多メンバ数を持つクラスタの要素をインライヤとして、最小自乗法によりモデル姿勢を決定するアフィン変換パラメータを推定したが、これに限定されるものではなく、例えば最多メンバ数を持つクラスタのセントロイドを、モデル姿勢を決定するアフィン変換パラメータとしても構わない。
実際のオブジェクト画像からのモデル抽出の一例を図16に示す。図16(A)はモデル画像、図16(B)はモデル画像から抽出された特徴点、図16(C)はオブジェクト画像、図16(D)はオブジェクト画像から抽出された特徴点、図16(E)は最終的な候補対応特徴点ペア、図16(F)はオブジェクト画像中のモデルの姿勢を示す。
(2−2)物体学習フェーズ
上述したロボット装置1が環境中を移動しながら未知の物体を自律的に学習する物体学習フェーズについてさらに説明する。この物体学習フェーズでは、ロボット装置1が学習対象となる物体を探索して接近し、腕部ユニット4R/L等の可動部を学習対象物体に接触させて動かすことで学習対象物体のモデル画像(後述する物体領域画像)を切り出し、そのモデル画像から特徴量(後述するモデル特徴量セット)を抽出して物体モデルデータベースに登録する。
物体学習フェーズにおける処理を図17A及び図17Bのフローチャートに示す。先ずステップS61において、後述するカウンタのカウント値OKcnt及びNGcntを0に初期化する。
次にステップS62において、ロボット装置1は、環境地図上の障害物のうちで既学習物体と対応付けられていない未学習の比較的小さな障害物、例えばロボット装置1が腕部ユニット4R/Lで動かせる程度の大きさの障害物を探索し、その障害物を学習対象物体として設定する。なお、未学習物体(障害物)が複数ある場合には、例えば現在位置から最も距離の近い未学習物体を選択するなど、何らかの指標に基づいて1つの未学習物体を学習対象物体として選択する。
続いてステップS63において、ロボット装置1は、学習対象物体の3次元位置を環境地図から算出し、ステップS64において、算出した3次元位置の少し手前、具体的には腕部ユニット4R/Lを動かして学習対象物体に接触させることができる位置を目的位置とし、目的位置までの経路を自動生成して、生成した経路に従って移動する。
続いてステップS65において、ロボット装置1は、環境地図から得られた学習対象物体の3次元位置の方向に方向転換し、カメラ画像内に学習対象物体が入り、且つ腕部ユニット4R/Lを動かせるように姿勢を変える。ここで、環境地図から得られる3次元位置情報は、学習対象物体を動かすための腕部ユニット4R/Lの制御に用いるほどには精度が高くないため、実際に腕部ユニット4R/Lを学習対象物体に接触させるには、左右のCCDカメラによるカメラ画像から得られたステレオ画像データに基づいて、学習対象物体の3次元位置をより精度よく推定する必要がある。そこでステップS65では、環境地図から得られた学習対象物体の3次元位置の値から所定の閾値の範囲内の値を持つカメラ画像上の単一領域を、ステレオ画像データから得られる3次元位置情報に基づいて抽出し、その画像領域の重心の3次元位置を学習対象物体に対する接触目標位置とする。
続いてステップS66において、ロボット装置1は、対応するアクチュエータA2,A3・・・を駆動制御し、腕部ユニット4R/Lを学習対象物体の接触目標位置に動かして接触させることで、学習対象物体を動かす。
続いてステップS67において、ロボット装置1は、学習対象物体を動かした際の動画像情報(時間的に隣接する2枚の静止画像)から動き領域の抽出を行う。ここで、ロボット装置1の頭部ユニット3に設けられた一対のCCDカメラからは、各時刻における左右のカメラ画像が得られる。そして、この左右のカメラ画像から得られたステレオ画像データからは、上述したように、各画素の距離値が導出可能である。現時刻と1つ前の時刻のカメラ画像の差分から輝度値の変化のあった画素、つまり差分画像が検出される。これらの画素は、動き領域のエッジ近傍に検出される傾向があり、動き領域の一部分のみがこの操作によって抽出されることになる。したがって、これらの画素に関して距離値を横軸、画素数を縦軸とするヒストグラムを作成した場合、大きいピークを与える距離dを持つ画素群が動き領域となる。そこで、ステップS67では、先の距離画像においてdから所定の閾値の範囲内の距離値を持つ画素を抽出し、領域面積の大きい連結領域のみを動き領域画像として抽出する。
このステップS67で抽出された動き領域画像には、学習対象物体とこの学習対象物体を動かしたロボット装置1の腕部ユニット4R/L、さらに学習対象物体が床面に接地している場合にはこの学習対象物体と距離が等しい床面の一部が含まれている。そこで、ステップS68において、ロボット装置1は、床面及びロボット装置1の部位に該当する領域をマスキングし、物体領域画像のみを抽出する。ここで、ロボット装置1は、ステレオ画像データから床面の検出が可能であるため、ステップS67で抽出した動き領域画像から床面領域をマスキングすることが可能である。また、ロボット装置1は、CCDカメラを搭載している頭部ユニット3、学習対象物体を動かした腕部ユニット4R/Lを含め、全関節角情報が観測でき、さらにロボット装置1の各リンク長も既知である。したがって、以上の情報とCCDカメラの光学的特性から得られた射影モデルとを用いて、学習対象物体を動かしたロボット装置1の腕部ユニット4R/Lが画像中のどの座標に現れているかを幾何学的計算により求め、ステップS67で抽出した動き領域画像からロボット装置1の腕部領域をマスキングすることが可能である。以上の方法により、学習対象物体が動いた場合には物体領域画像のみを抽出することが可能となる。
ここで、ステップS67,ステップS68における操作の例を図18に示す。図18(A)は前時刻における左のCCDカメラによるカメラ画像、図18(B)は現時刻における左のCCDカメラによるカメラ画像をそれぞれ示し、共にロボット装置1の腕部ユニット4Rにより学習対象物体SObを動かす様子が撮像されている。また、図18(C)は現時刻におけるレンジデータ、図18(D)は差分画像、図18(E)は差分画像に関する距離値のヒストグラムをそれぞれ示す。18(F)は、図18(E)に示すヒストグラムのうち、大きいピークを与える距離近傍の画素群、具体的には図中矢印で示す範囲の距離値を有する画素群を抽出した動き領域画像を示し、図18(G)はステップS68で抽出された物体領域画像を示す。
続いてステップS69において、ロボット装置1は、物体領域画像が抽出されたか否かを判別し、物体領域画像が抽出されなかった場合(No)には、ステップS70で物体領域画像抽出が不成功となった回数を保持するカウンタのカウント値NGcntをインクリメントしてステップS71に進む。ステップS71において、ロボット装置1は、NGcntが既定値Nngよりも大きいか否かを判別し、大きい場合(Yes)には処理を終了する。一方、ステップS71において、NGcntが既定値Nng以下である場合(Yes)には、ステップS72で例えば現在の目標位置をランダムに適当な大きさだけずらすなど、学習対象物体への接触目標位置の修正を行い、ステップS66に戻る。
一方、ステップS69において、物体領域画像が抽出された場合(Yes)には、ステップS73で物体領域画像抽出が成功した回数を保持するカウンタのカウント値OKcntをインクリメントすると共にカウント値NGcntを0に初期化してステップS74に進む。ステップS74において、ロボット装置1は、物体認識システム(図8)の特徴点抽出部310a及び特徴量保持部320aを用いて物体領域画像の特徴点及びその特徴量(以下、特徴点及びその特徴量を総称して「特徴量」という。)を抽出する。このように抽出された全ての特徴量は、特徴量セットとして短期メモリに保持される。
ここで、本実施の形態では、学習対象物体のより質の良いモデル、つまり認識に有効な特徴量セットを抽出するため、ステップS66からステップS74までの操作を繰り返し、各時刻における特徴量を時系列データメモリに保持する。そして、充分な繰り返しの後、時系列データに基づいて学習対象物体のモデルとなる特徴量セットを抽出し、物体モデルデータベースに登録する。
具体的に、ステップS75において、ロボット装置1は、カウント値OKcntが既定値N(例えばN=10)より大きいか否かを判別し、OKcntが既定値N以下である場合(No)には、ステップS76において例えば物体領域画像の重心の3次元位置を学習対象物体の接触目標位置とするなど、学習対象物体の移動後の接触目標位置を算出する。また、このステップS76において、ロボット装置1は、必要に応じて学習対象物体に接触するために方向転換や、姿勢変更を行う。一方、ステップS75において、OKcntが既定値Nより大きい場合(Yes)にはステップS77に進む。
以上の繰り返し処理により、時系列データメモリにはN枚の物体領域画像の特徴量セットが保持されている。そこでステップS77において、ロボット装置1は、この時系列特徴量セットから学習対象物体のモデルとなる特徴量セットを抽出し、ステップS78において、抽出されたモデル特徴量セットを物体モデルデータベースに登録する。
ここで、モデル特徴量セットの抽出は、時系列データメモリ内の各時刻の特徴量セット同士を全ての組合せで特徴量マッチングしたときに、マッチした、すなわち他の時刻における特徴量と類似し対応の取れる特徴量をより多く有する特徴量セットの中から、より多くマッチした特徴量のみを選択することで実現することができる。
以下、このモデル特徴量セットの抽出アルゴリズムについて説明する。時系列データメモリ内のN枚の物体領域画像の特徴量セットをF1,F2,…,FNとする。i=1,…,N、j=1,…,Nのi≠jである全ての組合せ(Fi,Fj)に関して、Fiをモデル画像の特徴量群、Fjをオブジェクト画像の特徴量群と仮に見なして物体認識システム(図8)の特徴量比較部330にかけ、(Fi,Fj)の候補対応特徴点ペア群を抽出する。そして、(Fi,Fj)の候補対応特徴点ペアの数をpijとし、j≠iである全てのjについてのpijの総和をPiとしたとき、Max{Pi}を満たすiを求め、以下ではFiに注目する。
具体的にN=5の場合、図19(A)に示すように、特徴量セットF1,F2,…,F5のi≠jである全ての組合せ(Fi,Fj)に関して候補対応特徴点ペア群を抽出する。例えば特徴量セットF4について、(F4,F1),(F4,F2),(F4,F3),(F4,F5)の組み合わせに関して候補対応特徴点ペア群を抽出する。この結果、候補対応特徴点ペアの数pijが例えばp41=3,p42=1,p43=2,p45=2となった場合、その総和P4は8となる。ここで、例えばp41=3とは、特徴量セットF4と特徴量セットF1との間の候補対応特徴点ペアが3組存在することを意味する。同様にして、P1=6,P2=5,P3=6,P5=7であった場合、その最大値を示すiは4であり、以下ではF4に注目する。
次に、Fiの特徴量をvi1,vi2,…,viLとする。各特徴量vil(l=1,…,L)が他の特徴点とペア組みされた回数をq_vilとし、q_vilが充分大きい特徴量vil、例えば以下の式(15)を満たす特徴量vilのみを抽出し、学習対象物体のモデル特徴量セットMとして物体モデルデータベースに登録する。
同様にN=5の場合、図19(B)に示すように、注目している特徴量セットF4の特徴量v41は、特徴量セットF1,F2,F3,F5の特徴点とペア組みされているため、q_v41=4となる。同様にして、q_v42=2,q_v43=3となった場合、上述した式(15)を満たす特徴量v41,v43を学習対象物体のモデル特徴量セットMとして物体モデルデータベースに登録する。
(2−3)物体認識フェーズ
上述したロボット装置1が環境中を移動しながら学習済みの物体を認識する物体認識フェーズについてさらに説明する。この物体認識フェーズでは、ロボット装置1の頭部ユニット3に設けられたCCDカメラで撮像した入力画像の特徴量を抽出し、その特徴量を物体モデルデータベースに登録されている既学習物体の特徴量と比較することで、入力画像中に存在する物体をリアルタイムに検出する。なお、この物体認識フェーズでは、上述した物体学習フェーズとは異なり、物体に腕部ユニット4R/L等の可動部を接触させて動かす必要はない。
物体認識フェーズにおける処理を図20のフローチャートに示す。先ずステップS81において、物体認識システム(図8)の特徴点抽出部310b及び特徴量保持部320bを用いて入力画像の特徴量セットFを抽出する。一方、物体モデルデータベースにはK個の既学習物体に対応するK個のモデル特徴量セットM1,M2,…,MKが登録されているとする。
次にステップS82において、物体認識システムの特徴量比較部330及びモデル姿勢推定部340を用いて、特徴量セットFと各モデル特徴量セットMiとのマッチングをし、入力画像中の物体Miの検出を行う。このとき、モデル特徴量セットMiの要素数、すなわち特徴量数をmiとし、特徴量セットMiのうち特徴量セットFと対応がとれた要素数をmfiとして、以下の式(16)に示すように、mfiのmiに対する比率を物体Miの検出結果の確信度Ciと定義する。
続いてステップS83において、入力画像中の既学習物体の認識結果を出力する。このとき、各既学習物体の認識結果は、確信度Ciと、mfiが3以上の場合は既学習物体の入力画像中での推定姿勢(アフィン変換パラメータ:既学習物体の平行移動量、回転角度、拡大率)により出力される。
この手法では、各既学習物体に対応するモデル特徴量セットMiが独立してマッチングにかけられるため、入力画像中に複数の既学習物体が存在する場合にも対応することができる。また、この確信度Ciを、例えば複数の物体がオーバーラップして検出された場合の状況判断に用いることができる。例えば、A及びBの2物体が共に高い確信度でオーバーラップして検出された場合には、一方が他方を部分的に隠していると判断できる。一方、Aのみが高い確信度を持つ場合には、Bの大部分がAによって隠されているか、或いはBが誤検出である可能性が高いと推定できる。また、A,B共に低い確信度の場合には、何れの物体も誤検出である可能性が高いと推定できる。このようにファジィな認識結果を出力することにより、この認識結果に基づいて行われるロボット装置1の行動選択などに柔軟性を与えることが可能となる。
(3)変形例
以上、本発明を実施するための最良の形態について説明したが、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
例えば、上述した実施の形態では、2足歩行型のロボット装置に適用する場合について説明したが、これに限定されるものではなく、この他種々の構成のロボット装置及びロボット装置以外の他の装置に広く適用することができる。
また、上述した実施の形態では、物体学習フェーズにおいて、ロボット装置1の腕部ユニット4R/Lを用いて学習対象物体を動かしたが、これに限定されるものではなく、例えばロボット装置の脚部、筐体、車輪部などで学習対象物体を動かすようにしても構わない。
また、上述した実施の形態では、障害物の存在確率を保持する環境地図上の未知小型障害物を学習対象物体とする場合を述べたが、これに限定されるものではなく、例えばVisualAttentionの技術(「Itti L., Koch C., Niebur E.,“A Model of Saliency-based Visual Attention for Rapid Scene Analysis,”IEEE PAMI, 20, 11, 1998, pp.1254-1259」参照)のように、カメラ画像中の色、テクスチャ、動き、エッジなどの情報から空間的、時間的に顕著な特徴を呈する点や領域を抽出し、その点を含む物体を学習対象物体とするようにしても構わない。
また、上述した実施の形態では、動領域を抽出する際に差分画像と距離情報とを用いるものとして説明したが、これに限定されるものではなく、例えばオプティカルフローを用いた手法(「岡田,白井,三浦,久野、『オプティカルフローと距離情報に基づく動物体追跡』、電子情報通信学会論文誌 D-II, Vol.J80-D-II, No.6, pp.1530-1538, 1997」参照)や、watershed法を用いた手法(「境田,苗村,金次、『背景差分法と時空間watershedによる領域成長法を併用した動画像オブジェクトの抽出』、電子情報通信学会論文誌 D-II, Vol.J84-D-II, No.12, pp.2541-2555, 2001」参照)など、種々の動領域抽出手法を適用することができる。
また、上述した実施の形態では、物体認識の基本技術として特許願2003−124225の明細書及び図面で提案した手法をベースとした場合について説明したが、これに限定されるものではなく、種々の物体認識システムを利用することができる。この場合、物体モデルデータベースには、その物体認識システムが利用する特徴量セットを保持すればよい。また、候補対応特徴点ペア群を出力しない物体認識システムを利用する場合には、図17のステップS66乃至ステップS74の繰り返しを1回のみにすることで本発明の適応が可能となる。
1 ロボット装置、2 体幹部ユニット、3 頭部ユニット、4R/L 腕部ユニット、5R/L 脚部ユニット、300 物体認識システム、310a,310b 特徴点抽出部、320a,320b 特徴量保持部、330 特徴量比較部、340 モデル姿勢推定部