JP2005092820A - 環境認識装置及び方法、経路計画装置及び方法、並びにロボット装置 - Google Patents

Abstract

【課題】床面とは段差があるような領域であっても移動可能であるか否かを判定するための環境地図を作成することができる環境認識装置及び方法、そのような環境地図を用いて適切に移動経路を計画できる経路計画装置及び方法、並びに環境認識装置及び経路計画装置を搭載したロボット装置を提供する。
【解決手段】視差画像又は距離画像から平面パラメータを算出して床面を含む複数平面を抽出する複数平面抽出部４０１、床面を含む複数平面の障害物を認識する障害物認識部４０２、及び障害物認識部４０２の認識結果に基づき各平面毎の環境地図（障害物地図）を作成及び更新する環境地図更新部４０３からなる環境認識部と、これらの環境地図に基づき経路計画を行う経路計画部４０４とを有する。経路計画部４０４は、床面の環境地図上で障害物とされた場合であっても、他の平面における環境地図上で障害物ではないとされている場合は、移動経路の候補とする。
【選択図】図５

Description

本発明は、障害物が存在する環境下において視差又は距離画像等の３次元データから平面を抽出し、抽出された平面の障害物を認識する環境認識装置及び方法、この環境認識により得られた環境地図に基づき現在位置から目的位置まで移動するための経路を計画する経路計画装置及び方法、並びに、この環境認識装置又は経路計画装置を搭載したロボット装置に関する。

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

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

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

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

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

特許第３３４０５９９号公報

しかしながら、移動型のロボット装置においては、このようにして平面を床面として検出した場合、例えば床面上にある物体等は障害物とし、これを回避するような経路計画が立てられていた。ここで、自律に移動するロボット装置においては、例えば床面より高さが高い物体等を障害物としてしまうと、階段等の本来移動可能である領域も障害物として移動不可能にと判断してしまうという問題点があった。例えば階段を登る機能を有するような移動型のロボット装置においては、予め階段の位置、形状及びそれが移動可能であるという情報を入力しておかなければ、障害物と認識してしまう。

本発明は、このような従来の実情に鑑みて提案されたものであり、床面とは段差があるような領域であっても移動可能であるか否かを判定するための環境地図を作成することができる環境認識装置及び方法、そのような環境地図を用いて適切に移動経路を計画できる経路計画装置及び方法、並びに環境認識装置及び経路計画装置を搭載したロボット装置を提供することを目的とする。

上述した目的を達成するために、本発明に係る環境認識装置は、障害物が存在する環境下において移動型のロボット装置が環境を認識するための環境認識装置において、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出手段と、上記平面検出手段により検出された平面上の障害物を認識する障害物認識手段と、上記障害物認識手段により認識された障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新手段とを有し、上記平面検出手段は、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、上記環境地図更新手段は、上記平面群のうち、上記障害物認識手段により障害物が認識された平面の環境地図を作成及び／又は更新することを特徴とする。

本発明においては、例えば床面等の基準となる一の平面のみならず、基準平面に平行な複数の平面を検出し、各平面における環境地図を作成又は更新することで、移動型ロボット装置が３次元的に環境を認識するための環境地図を構築することができる。

また、上記基準平面は、床面又は上記ロボット装置が移動している移動面とすることができ、移動型ロボット装置が床面を移動しているときは、床面を基準とし、階段を登っている途中などにおいては、その移動中の移動面を基準とすることができる。

更に、上記各平面は、上記ロボット装置が移動可能な面積を有し、また、上記基準平面との高さの差が所定の値以下である平面としたり、上記他の平面は、上記ロボット装置の認識範囲内において上記基準平面と高さの差が最も小さい平面を含むようにすることで、例えばロボット装置の移動可能な範囲の高さの平面を抽出するようにすれば、この平面が移動経路となる可能性が高くなる。

本発明に係る環境認識方法は、障害物が存在する環境下において移動型のロボット装置が環境を認識するための環境認識方法において、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出工程と、上記平面検出工程にて検出された平面上の障害物を認識する障害物認識工程と、上記障害物認識工程にて認識された障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新工程とを有し、上記平面検出工程では、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、上記環境地図更新工程では、上記平面群のうち、上記障害物認識手段により障害物が認識された平面の環境地図を作成及び／又は更新することを特徴とする。

本発明に係る経路計画装置は、障害物が存在する環境下において移動型のロボット装置が現在位置から目的位置まで移動するための経路を計画する経路計画装置において、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出手段と、上記平面検出手段により検出された平面群における各平面上の障害物を認識する障害物認識手段と、上記障害物認識手段により認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新手段と、上記環境地図更新手段により作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画手段とを有することを特徴とすることを特徴とする。

本発明においては、複数の平行な平面を抽出して各平面における障害物情報を取得し、これにより得られた３次元的な環境地図を示す環境地図群を保持しており、この複数の環境地図に基づき移動経路を計画するため、一の環境地図に基づき計画する経路計画に比して移動の範囲が広がる。

また、上記経路計画手段は、各環境地図を所定の領域に区切り、上記ロボット装置の現在位置から移動可能とされる各領域毎に該各領域に移動する際のコストを算出し、コスト算出結果に基づき移動経路を計画することができ、例えば最もコストが低い経路を選択して移動する等することができる。

更に、上記環境地図は、障害物が存在するとされる障害物領域と、移動可能であるとされる自由空間領域と、未観測である未観測領域とを有し、上記経路計画手段は、現在移動中の平面に対応する環境地図である第１の環境地図上に上記障害物領域が含まれる場合、上記移動中の平面とは異なる平面に対応する環境地図である第２の環境地図を参照し、当該障害物領域が該第２の環境地図上で自由空間領域である場合は、当該障害物領域を移動可能領域とみなすことができ、床面のみの環境地図では、移動不可とされていた領域も複数平面の環境地図を保持し、これらを参照することにより移動経路の候補することができる。

本発明に係る経路計画方法は、障害物が存在する環境下において移動型のロボット装置が現在位置から目的位置まで移動するための経路を計画する経路計画方法において、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出工程と、上記平面検出工程にて検出された平面群における各平面上の障害物を認識する障害物認識工程と、上記障害物認識工程にて認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新工程と、上記環境地図更新工程にて作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画工程とを有することを特徴とする。

本発明に係るロボット装置は、障害物が存在する環境下で自律的に移動可能なロボット装置において、上記環境を認識するための環境認識装置を有し、上記環境認識装置は、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出手段と、上記平面検出手段により検出された平面上の障害物を認識する障害物認識手段と、上記障害物認識手段からの障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新手段とを有し、上記平面検出手段は、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、上記環境地図更新手段は、上記平面群のうち、上記障害物認識手段により障害物が認識された平面の環境地図を作成及び／又は更新することを特徴とする。

本発明に係るロボット装置は、障害物が存在する環境下にて現在位置から目的位置まで移動するための経路を自律的に計画して移動するロボット装置において、上記環境を認識して移動経路を計画する経路計画装置を有し、上記経路計画装置は、入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出手段と、上記平面検出手段により検出された平面群における各平面上の障害物を認識する障害物認識手段と、上記障害物認識手段により認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新手段と、上記環境地図更新手段により作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画手段とを有することを特徴とする。

本発明の環境認識装置及び方法によれば、複数の平行平面からなる平面群を抽出し、各平面群の障害物を認識した障害物情報に基づき各平面の障害物占有率を有する環境地図群を構築することができ、障害物が存在する環境下で自律的に移動可能なロボット装置が、環境を３次元的に把握することができる。

また、本発明に係る経路計画装置及び方法によれば、そのような３次元的に環境を把握可能な環境地図群を参照して移動経路を立てることができ、現在位置から目的位置まで移動するための経路候補となり得る領域が広がり、例えば階段を登ったりする経路計画を立てることができる。

更に、本発明に係るロボット装置によれば、床面等の基準となる平面とこれに平行な複数の平面群から得られた環境地図群により、障害物が存在する環境を３次元的に把握することができ、更にこれを利用して移動経路を計画すれば、現在移動している面を基準とした環境地図のみでは障害物ありとされ移動不可とされている領域であっても、他の環境地図を参照して移動経路を計画することができ、外部からの指示がなくても階段等が移動経路となり得て自律的階段を登ったりする動作を発現することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、複数の平面を抽出してそれらの平面の障害物情報を有する環境地図を構築する環境認識装置、及びこの環境認識装置により構築された環境地図に基づき移動経路を計画する経路計画装置を搭載し、自律的に移動可能なロボット装置に適用したものである。

本実施の形態における環境認識装置は、例えばこれを搭載したロボット装置の脚部が接している面（on foot plane）等の基準となる基準平面を抽出し、この基準平面と例えば平行な複数の平行平面を抽出する。そして、この環境認識装置により、複数の平行平面における後述する障害物占有率等を算出して環境地図を構築することで、例えばこれを搭載した脚式移動型のロボット装置は、階段等の段差を障害物か又は移動可能かを判断し、段差のある領域等を障害物とせず、経路候補として移動経路の計画を行うことができるものである。

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

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

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

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

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

次に、図２に示すロボット装置１のソフトウェアの動作について説明する。図３は、図２に示すソフトウェア３００の動作を示すフローチャートである。

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

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

また、センサデータＤ４から得られる歩行パラメータと、足底センサを使用した歩数のカウント数とからロボット装置１の移動量を算出し、ロボット装置１のロボット装置中心座標系における移動量を算出する。以下、ロボット装置中心座標系の移動量をオドメトリともいう。このオドメトリＤ７をグリッド占有率算出部ＯＧ３３０及び絶対座標算出部ＬＺ３５０へ出力する。そして、複数平面抽出部ＰＬＥＸ３２０及びグリッド占有率算出部ＯＧ３３０にて障害物のグリッド占有率（環境地図）が算出、更新され、ランドマーク位置検出部ＣＬＳ３２０及び絶対座標算出部ＬＺ３５０では絶対座標が算出、更新される（ステップＳ４、５）。

以上のステップＳ４、５に示す複数平面抽出部ＰＬＥＸ３２０及びグリッド占有率算出部ＯＧ３３０の処理と、ランドマーク位置検出部ＣＬＳ３２０及び絶対座標算出部ＬＺ３５０の処理とは、同時並行的に行われるものである。

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

更に、本実施の形態における平面抽出部ＰＬＥＸ３２０は、後述する如く、床面のみならず、床面と、この床面に平行な複数の他の平面からなる平面群を抽出し、床面及び複数の各他の平面において、各他の平面上にない点、例えば所定の閾値よりも高い位置にあるものを同じく障害物として上記各平面からの距離を算出し、この障害物情報も合わせてグリッド占有率算出部３３０に出力する。ここで、ロボット装置が移動している例えば床面等の移動面を基準平面として、この基準平面と高さの差がなるべく小さい平面を他の平面として抽出するようにすることで、ロボット装置において移動候補の可能性を有するような有効な平面を抽出するようにする。即ち、例えば車輪により移動したり、脚部を有して２足歩行又は４足歩行をする移動型ロボット装置において、高さ方向に移動不可能な平面を抽出するより、高さ方向の差が小さく移動可能であるような平面を優先的に抽出することで、後述する環境地図等を作成し、これに基づき経路計画を実行する際に有効に利用することができる。なお、基準平面に距離的に近い平面を抽出するようにしてもよい。

こうして、基準平面と、これに平行な複数の平面からなる平面群を抽出し、各平面において障害物が認識された場合に、その障害物情報をグリッド占有率算出部３３０に出力する。なお、以下では、床面等の基準平面に平行な平面を抽出するものとして説明するが、平面群には、床面に平行な平面のみならず、坂道のように若干傾斜のあるような平面を含むようにしてもよい。

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

更に、床面の障害物のグリッド占有率と同様、床面以外の平面群に含まれる他の各平面の障害物のグリッド占有率を確率的手法により更新する。

このグリッド占有率算出部ＯＧ３３０は、ロボット装置１を中心とした例えば周囲４ｍの障害物情報（障害物のグリッド占有率）を示す環境地図と、ロボット装置１が向く方角を示す姿勢情報とを保持しており、上述の方法により、環境地図を更新し、この更新した認識結果（障害物情報Ｄ８）を出力することにより、上位レイヤ、即ち、本実施の形態においては、経路計画決定部ＳＢＬ３６０にて障害物を回避する計画を作成することができる。ここで、グリッド占有率算出部ＯＧ３３０は、上述したように、平面抽出部ＰＬＥＸ３２０から複数平面及びその障害物情報を受け取っており、したがって各平面における環境地図を作成又は更新する。

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

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

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

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

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

ここで、本実施の形態におけるロボット装置は、平面抽出部ＰＬＥＸ３２０にて床面を含む複数の平行平面からなる平面群を検出し、これら複数の平面についての障害物占有率を示す環境地図を構築し、各平面に対応した複数枚の環境地図からなる環境地図群に基づき経路計画を行うことで、床面のみの環境地図を有する場合に比して行動可能範囲を広め、例えば階段等の段差を有する領域を移動経路に含めることで、移動を可能とするものである。すなわち、ロボット装置が経路計画を実行する際、現在移動している例えば床面に対応する環境地図のみならず、この床面に平行な平面に対応する環境地図を参照することを許すことで、３次元的に環境を把握し、したがって３次元的な移動経路を計画することができるものである。

以下、複数の平行平面を抽出して経路計画を実行可能な本実施の形態のロボット装置について詳細に説明する。このロボット装置における複数平面抽出から経路計画を行う処理を機能的に分類すると、図５に示すように、視差画像又は距離画像から平面パラメータを算出して床面を含む複数平面を抽出する複数平面抽出部４０１、床面を含む複数平面の障害物を認識する障害物認識部４０２、及び障害物認識部４０２の認識結果に基づき環境地図（障害物地図）を作成及び更新する環境地図更新部４０３からなる環境認識部と、環境地図に基づき経路計画を行う経路計画部４０４とを有する。なお、ここでは、機能毎のブロックとして示すが、以上の一連の処理は上述したように、図２に示す各オブジェクト単位で処理が行われる。

複数平面検出部４０１及び障害物認識部４０２は、同次変換行列と、これに対応する図６に示す左目画像２０１Ｌ及び右目画像２０１Ｒから得られた視差画像を受け取り、図７に示す処理手順に従って、平面パラメータを検出し、障害物を認識する。

平面パラメータの検出は、先ず複数平面検出部４０１が同次変換行列とこれに対応する視差画像を受け取る（ステップＳ３１）。例えば、ステレオ視（ステレオカメラ）等による方法、又はレンジセンサ等の操作によって得られた視差画像３１２（又は距離画像）は、適切なキャリブレーションに基づく変換により、３次元データに容易に変換することができ、この３次元データの配列が生成される。即ち、視差画像からレンズ歪みとステレオの取り付け誤差を吸収したキャリブレーションパラメータを利用して、画素毎にカメラ座標から見た３次元の位置データ（ｘ，ｙ，ｚ）を距離画像として生成する（ステップＳ３２）。このとき各３次元データは、視差画像又は距離画像等の入力画像における信頼度等から得られる信頼度パラメータを個別に有し、この信頼度パラメータにより選別されて入力される。

この選別された３次元データ群からランダムにデータをサンプリングし、ハフ変換により平面を推定する。即ち、法線ベクトルの向きを（θ，φ）、原点からの距離をｄとしたときの平面パラメータ（θ，φ，ｄ）を算出し、平面パラメータを投票空間（θ，Ψ，ｄ）＝（θ，φｃｏｓθ，ｄ）に直接投票して平面を推定する。これにより、画像中の支配的な平面のパラメータを検出する（ステップＳ３３）。

ここで、ステップＳ３３における平面パラメータを検出する方法について詳細に説明する。複数平面検出部４０１は、ロボット装置の脚部ユニットが接している又は移動している例えば床面等の移動面（以下、基準平面という。）のみならず、基準平面と平行な複数の平面を示す平面パラメータを抽出する。図８は、平面パラメータを検出する方法を示すフローチャートである。上述したように、各３次元パラメータは個別に信頼度パラメータが算出され、この信頼度パラメータにおいて信頼度が所定の閾値より低いデータは切り捨てる等して入力データの選別が行われる。そして、選別された３次元データ群（ｘ，ｙ，ｚ）及び各データに付随する信頼度パラメータ（信頼度ｒ）が出力され（ステップＳ４１）、これらのデータ群から、平面パラメータを推定するためのサンプルを抽出するサンプリングを行い、平面パラメータを推定する（ステップＳ４２）。

サンプリングの方法としては、３次元データ（ｘ，ｙ，ｚ）から
１．全くランダムにサンプリングする方法
２．基準点となる１点を３次元データ全体からランダムにサンプリングし、残りのデータを基準点近傍点、例えば３次元空間内での距離が十分近いデータ群等からサンプリングする方法
３．３次元データを複数の解像度レベルに亘ってスキャンしながら各小近傍（例えば、３次元空間内での距離が十分近いデータ群等）のデータをひとまとまりにしてサブセットとする方法
等がある。

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

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

次に、抽出したデータサブセットから平面パラメータを算出する。ここでは、例えば３種類の平面パラメータの算出方法から、必要に応じて最適な算出方法をとることができる。

即ち、第１のパラメータ算出方法は、同一直線にない３点の３次元データから直接解法を使用して、法線ベクトルの向き（θ，φ）と座標原点からの距離ｄという３つのパラメータ（θ，φ，ｄ）により決定される平面αを求めることができる。

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

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

次に、このようにして得られた平面パラメータをパラメータ空間に投票する（ステップＳ４３）。この際、平面を指定する平面パラメータは、元の３次元直交座標空間に対して極座標を形成することから、投票スロット（グリッド）が元の対応空間内で同一の大きさとなるように、例えば（θ,Ψ，ｄ）＝（θ，φｃｏｓθ，ｄ）又は（θ,Ψ，ｄ）＝（２θsin（π/４−θ/２）cosφ，２sin（π/４−θ/２）sinφ，ｄ）等として投票空間を設計することにより、更にロバストで精度が高いデータが得られ、従ってサンプリング数を減らすことができ、処理が高速化する。また、一般に、投票の際の１票の重みは同じである場合が多く、例えば一の投票に対しその投票値を１とする等とされるが、誤差の性質を考慮して一票の重みを定めるれば、ノイズに対するロバスト性を向上することができる。即ち、一の投票に対し、上述の信頼度パラメータ、投票先を計算するために使用した元の３次元データの誤差の標準偏差、又は計算元の３次元データの分布を視点中心の球面に投影したときの立体角等を使用した誤差の評価によって、その投票値を例えば０．５又は２等とするものである。

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

この平面のパラメータは図９に示すパラメータ空間（θ，φ）（投票空間）のヒストグラムにより検出することができる。パラメータの投票が小さいいと障害物、大きいと平面と見なすことができる。

次にピーク値近傍における重み付け平均処理が行われる（ステップＳ４５）。この重み付き平均処理により、ピークのより正確な位置を推定することができる。これにより、各グリッドの代表パラメータの量子化サイズよりも細かい粒度でのパラメータ推定が可能となる。

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

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

以上説明したように、投票の際は、３次元データに付随する信頼度パラメータ又は平面パラメータの算出方法等により、１票の投票に対して異なる重み付けをして、投票値を異ならせると共に、投票値の分布から得られるピーク値を推定することで、ピーク値近傍における重み付き平均処理等をすることができ信頼度の高いデータを推定することができる。そして、この平面パラメータを初期パラメータとしてイタレーションを行い、平面を決定することで、更に信頼度が高い平面を決定することができる。更に、最終的に決定される平面を算出した３次元データに付随する信頼度パラメータ及びイタレーションにおける残差誤差等を使用して平面の信頼度を算出し、この平面信頼度を平面のデータと共に出力することにより、後段の処理が行い易くなる。このように、平面抽出は、投票、即ちヒストグラムに基づく確率密度関数の推定により３次元データから、その３次元データに含まれる支配的な平面のパラメータを決定する確率論的方法で行われる。こうして得られた平面のパラメータを使用すれば、元々画像から得られた距離の測定点が平面上からどれくらいの距離であるかが分かる。なお、以上は、平面抽出方法の一具体例であり、その他、装置の精度、性能等に応じて、他の公知の平面抽出方法にて平面抽出を行うようにしてもよい。

ここで、本実施の形態における複数平面検出部４０１は、上述したように、床面のみならず複数の平面を同時に抽出する。図１０は、角度及びオフセット量に対応した複数平面検出部４０１が検出する平面パラメータ（θ，Φ）によるヒストグラムとこれに対応する複数平面を示す模式図である。

図１０に示すように、上述のようにして投票により得られる平面パラメータのヒストグラムにおいて、ピークＨ_１に示す床面ＰＬ_１が対応する場合、ピークＨ_２、Ｈ_３を有する床面ＰＬ_１に平行なそれぞれ平面ＰＬ_２、ＰＬ_３をを抽出することができる。

そして、後述する障害物認識部４０２にて、床面等の基準平面より所定の閾値以上高い部分は障害物として認識するが、これら基準平面に平行な複数の平面を抽出することで、床面より高さが高いか又は低い部分であって平面である領域を障害物ではなく、他の平面として認識することができる。更に、認識された他の平面毎に、障害物情報を有する環境地図を作成しておくことにより、後述する経路計画の際の移動経路の候補とすることができる。

即ち、図７に戻って、カメラ座標系の同次変換行列から図１１に示すようにロボット装置の足底接地面への変換を求める（ステップＳ３４）。これにより、カメラ座標系で表現される接地平面ＰＬのパラメータの算出が行われる。そして、前記ステップＳ３３での画像による平面の検出と、前記ステップＳ３４での足底接地平面ＰＬとの照合から、画像中の平面パラメータから床面と、これに平行な複数の平面を選択する（ステップＳ３５）。本ロボット装置は、このような複数の平面を認識して、各平面毎に環境地図を保持することで、段差を有するような領域間の移動、例えば階段等を登ったり降りたりすることを可能とするものであり、このステップＳ３５にて検出する床面とは、ロボット装置が現在、接地又は移動している平面を示すこととする。

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

図１２には、複数選択された平面群のうち、床面において、しきい値Ｄ_ｔｈを１ｃｍとした範囲で選択された測定点（×印）を示す例である。この図１２にて、黒く抜かれている点が平面と判断されなかった点である。

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

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

この障害物認識部４０２は、多数の測定点を使って平面の検出を行うために安定した平面を抽出できる。また、画像から得られる平面候補をロボット装置の姿勢から得られる床面パラメータと照合することで正しい平面を床面として選択でき、更にこれに平行な複数の平面を選択することができる。このことにより、後述するように、床面に平行な平面であれば、ロボット装置にとって、例えば移動可能な領域と判断することができる。即ち、床面のみの検出では、段差のある領域等を障害物と見なし、移動不可能の領域と判断されていたが、ロボット装置が現在接地している面（床面）を移動可能領域とした環境地図において移動不可能領域であっても、他の環境地図において平面（移動可能な領域）とされている場合には、当該移動不可能領域を移動可能な領域として移動経路の候補とすることでロボット装置の移動範囲を広くすることができる。

また、障害物を認識するのではなく、実質的には床面及びその他の平面を認識するので障害物の形状や大きさに因らない認識が可能である。また、床面からの距離で障害物を表現するので細かい段差や凹みまで検出することができる。またロボット装置の大きさを考慮して跨いだり、くぐったりする判断も容易である。さらに、２次元床面上の障害物表現となるので経路計画などに既存の移動ロボット装置で用いられる手法が適用可能であるし、３次元の表現より高速に演算できる。

環境地図更新部４０３は、所定の大きさのグリッドに分割されたロボット中心座標系の地図情報である環境地図と、例えば環境地図上においてｘ軸方向又はｙ軸方向等の所定の方角からロボット装置が向く方角を示す姿勢情報とを有している。ここで、環境地図は、複数平面抽出部４０１により抽出された床面等の基準平面と、これに平行な複数の平面に対応する複数枚の環境地図であり、例えば周囲４ｍ四方の地図であり、これらが例えば４ｃｍ×４ｃｍ等の所定の大きさのグリッド毎に、障害物認識部４０２から入力される障害物情報により、障害物の占有率が算出される。

この環境地図更新部４０３は、ロボット装置１が移動すると、即ち、オドメトリＤ７が入力されると、予めメモリ内に格納されていた環境地図及び姿勢情報を、ロボット装置の姿勢の変化（差分移動角）及び移動距離（差分移動量）に伴って、予め認識している環境地図及び姿勢方向の情報を更新する。ここで、環境地図を所定の領域に区切っておくことで、例えば差分移動量が上記所定の領域サイズより小さければ環境地図を更新せず、移動量がグリッドの上記所定の領域サイズを超える場合は環境地図を更新するようにすると、環境地図更新の際の無駄な計算コスト及びメモリコピーコストを削減することができる。また、環境地図、及び障害物占有率を算出するためのグリッドの大きさを必要に応じて適宜変更する等することにより、計算量及びメモリコピーコスト等を低減してもよい。

この環境地図更新部４０３にて、障害物認識部４０２から供給される障害物情報により生成される環境地図上の点は、上述した各グリッドの障害物占有率に基づき図１４に示すように以下の３つの種類（領域）に分類することができる。

第１の領域は、障害物の存在する領域（図中の黒色で表される領域、以下障害物領域という。）Ａ_Ｏである。第２の領域は、自由空間（障害物が存在しない空間）上の領域（図中の白で表される領域、以下自由空間領域という。）Ａ_Ｆである。そして、第３の領域は、まだ障害物の観測がなされていない未観測領域（図中の斜線で表される領域）Ａ_Ｕである。

環境地図更新部４０３は、環境認識部４０２が認識した障害物情報に基づき、各平面毎に環境地図を作成し、又は更新する。例えば、図１５（ａ）に示すように、ロボット装置１が、階段５００を認識可能な範囲に位置している場合において、先ず、移動目的地等の所定の方向を観測する。これにより、平面検出部４０１にて床面ＰＬ_１と、これに平行な平面として、階段面ＰＬ_２、ＰＬ_３を検出することができる。障害物認識部４０２において、これら３つの面ＰＬ_１〜ＰＬ_３上の障害物情報を取得し、図１５（ｂ）に示すように、各面ＰＬ_１〜ＰＬ_３に対する環境地図５０１〜５０３をそれぞれ作成、又は更新する。

この場合、床面ＰＬ_１に対する環境地図５０１においては、階段５００は床面より高さｈ_１又は高さｈ_２だけ高い平面であるため、障害物領域Ａ_Ｏとされる。また、階段面ＰＬ_２に対する環境地図５０２においては、階段面ＰＬ_３は階段面ＰＬ_２から高さが（ｈ_２−ｈ_１）だけ高く、床面ＰＬ_１は階段面ＰＬ_２より高さがｈ_１低いため、いずれも障害物領域Ａ_Ｏとされる。同様に、階段面ＰＬ_３に対する環境地図５０３においても、床面ＰＬ_１及び階段面ＰＬ_２は、階段面ＰＬ_３より低いため、障害物領域Ａ_Ｏとされる。

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

次に、この経路計画部４０４が採用する経路計画アルゴリズムを図１６のフローチャートに示し、以下に詳細を説明する。

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

次に、生成された環境地図に基づき経路計画を行う（ステップＳ５３）。ここで、床面のみの環境地図しかもたないようなロボット装置の場合は、未観測領域Ａ_Ｕ及び自由空間領域Ａ_Ｆを移動可能領域とみなして経路計画を行うが、本実施の形態のロボット装置においては、上述したように、複数平面を検出して、これらの環境地図を作成、更新したものを保持している。例えば、ロボット装置が現在移動している平面を床面とする場合、床面より高さが高いような領域は全て障害物領域Ａ_Ｏとされるが、現在位置Ｐ_０から、途中経路位置Ｐ_１に進んだ後、この障害物領域Ａ_Ｕ１が障害物であるか、又は他の平面として検出されているかの判断を行うことができる。

そして、床面環境地図において障害物領域Ａ_Ｕ１とされているものの、他の平面として検出されている場合、当該他の平面に対応する環境地図である他の環境地図を参照し、この他の環境地図において、その領域Ａ_Ｕ１が自由空間領域Ａ_Ｆ又は未観測領域Ａ_Ｕである場合には移動可能領域とみなす。ここで、移動可能領域と見なされた領域Ａ_Ｕ１方向に移動するか、又はこれを迂回するような経路計画を実行するかは、ロボット装置に依存する。すなわち、例えばロボット装置は、移動可能領域の全てに対して移動の際のコスト計算をし、最もコストが低くなるよう経路計画を立てることができる。

図１７乃至図１９は、ロボット装置１により実際に保持される環境地図を時系列で示すものである。図１７乃至図１９に示すように、ロボット装置１が移動すると共に新たな障害物情報が得られ、環境地図が更新されて移動経路が設定されていく。これらの環境地図は、上述したように、ロボット装置１を中心に、前方、後方、左右両方向が例えば各２ｍの大きさを有したものとなっており、例えば縦１００×横１００個に仕切ったグリッド毎にその障害物の占有率が算出されたものとなっている。ロボット装置１は、これらの環境地図に基づき、例えば図１７乃至図１９のそれぞれ４１０乃至４１２に示すような計画経路を立てることができる。

また、図１４においては、障害物領域Ａ_Ｏは黒、自由空間領域Ａ_Ｆは白で示されるものとして説明したが、実際には、この図１７乃至図１９に示すように、周囲のハッチして示す領域が未観測領域Ａ_Ｕであり、その他の領域が観測済みの領域であって、白く示す領域が平面とされロボット装置１が移動可能な自由空間領域Ａ_Ｆであり、黒っぽくなるほど障害物の占有確率が高くなっていることを示す。すなわち、黒〜白の濃淡により、障害物の占有率の高さを示すものとなっている。このような濃淡で示された領域は、例えば所定の輝度未満であれば、自由空間領域Ａ_Ｆとし、それ以外は障害物領域Ａ_Ｏとしてもよく、又はこれらの濃淡の度合いに応じたコスト計算をするようにしてもよい。

ロボット装置１の実行する経路計画としては、このような環境地図において、現在の位置から移動可能な全てのグリッドに対して、移動するためのコストを個別に算出し、最もコストが小さいグリッドを移動経路として選択する等すればよい。

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

ここで、移動可能か否かの評価は、ロボット装置に因るものであるが、例えばロボット装置が登ることができる限界の高さより高い場合には、領域Ａ_Ｆが平面であっても、移動することができず、また、領域Ａ_Ｆが上記限界の高さ未満であっても、領域Ａ_Ｆがロボット装置が移動できるほど広くないような場合にも移動しないよう、最適なパラメータが設定されているものとする。また、他の全ての環境地図においても自由空間領域Ａ_Ｆ又は未観測領域Ａ_Ｕでもなく、完全な障害物領域Ａ_Ｏであるとされた場合には、極めて高いコストが算出され、ロボット装置の移動経路の候補となり得ないようになされている。

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

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

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

以上のような経路計画アルゴリズムを採用した経路計画部４０４は、未観測領域Ａ_Ｕ及び自由空間領域Ａ_Ｆのみならず、床面と平行な他の平面が存在する場合には、床面の環境地図における障害物領域Ａ_Ｏであっても、他の平面の環境地図を参照して移動可能領域か否かを判断して経路計画を行うことで、床面のみの環境地図を有する場合に比して移動可能範囲を格段に広くすることができる。また、出力された経路上に含まれる未観測領域部Ａ_Ｕのみを再観測することにより、目的地に移動する際に不必要な観測及び距離画像計算処理を行なわず、効率的で短時間に移動経路計画を生成することが可能となる。

また、ここでは、床面等の基準平面を抽出し、この基準平面と略平行な複数の平面を抽出するものとして説明したが、例えば坂道のような傾斜のある平面であっても、基準平面との傾きの度合いが小さくロボット装置が移動可能である場合には同様に平面として抽出し、障害物情報を獲得し、環境地図を作成、更新するようにしてもよい。

以上のようなコスト計算に基づき階段を登る脚式移動型のロボット装置について説明する。図２０は、階段を登る動作を行うロボット装置を示す模式図である。また、図２１は、図２０に示すような階段を登る動作を行うロボット装置の環境地図を示す図である。以下の説明においては、ロボット装置が移動している現在の移動面（ロボット装置の脚部が載っている平面：on foot plane）を基準平面ＰＬとする。また、床面ＰＬ_０上には、床面ＰＬ_０からの高さ（段差）がｈ_１〜ｈ_４のそれぞれ階段面ＰＬ_１〜ＰＬ_４が形成されているものとする（０＜ｈ_１＜ｈ_２＜ｈ_３＜ｈ_４）。

ロボット装置１は、基準平面ＰＬ＝床面ＰＬ_０に接地しているものとし、この床面ＰＬ_０から階段面ＰＬ_１〜ＰＬ_３を登る動作を行うこととする。先ず、ロボット装置１の平面検出部４０１は、床面ＰＬ_０から周囲を観察して、床面ＰＬ_０を検出し、この床面ＰＬ_０に平行な平面があればこれを抽出する。ここで、認識可能な全範囲に含まれる床面ＰＬ_０に平行な平面全てを抽出してもよいが、例えば床面ＰＬ_０からの高さ方向の差が小さいものから２つを選択することで、メモリ容量が限られたロボット装置の記憶する環境地図の数を制限することができる。図２０に示す場合においては、床面ＰＬ_０からの高さがｈ_１、ｈ_２の階段面ＰＬ_１、ＰＬ_２を選択するものとする。これにより、演算量を低減することができ、また、例え床面ＰＬ_０に平行であっても、段差が大きすぎて登れない可能性があるような平面を抽出しないようにすることができる。

そして、抽出した床面ＰＬ_０〜ＰＬ_２において、障害物認識部４０２により障害物が認識され、環境地図更新部４０３が認識された障害物情報に基づき、各平面における障害物占有グリッドが算出され、図１４に示すような環境地図を各平面ＰＬ_０〜ＰＬ_２について作成する。即ち、図２１に示す環境地図５１１〜５１３が作成される。

床面ＰＬ_０を移動しているロボット装置１では、先ず図２１に示す床面ＰＬ_０の環境地図５１１に基づき経路計画がなされる。ここで、床面ＰＬ_０の環境地図５１１においては、段差を有する階段面ＰＬ_１等は障害物領域Ａ_Ｏとされるが、階段面ＰＬ_１の環境地図５１２においては、環境地図５１１の障害物領域Ａ_Ｏのうち一部が平面ＰＬ_１であり、したがって自由空間領域Ａ_Ｆとされている。ロボット装置１は、この他の平面として検出されている階段面ＰＬ_１の環境地図５１２を参照して、環境地図５１１では障害物領域Ａ_Ｏとされている階段面ＰＬ_１へ移動可能と判断することができる。ここで、ロボット装置１は、上述したように、現在ロボット装置が位置する領域（グリッド）から隣接する領域（グリッド）へ移動する際のコストを算出する。床面ＰＬ_０の環境地図のみの場合は、環境地図５１２にて自由空間領域Ａ_Ｆとされる領域は障害物領域Ａ_Ｏとされるため、この領域への移動コストが非常に高くなり、ロボット装置１は移動不可と判断してしまうが、このように、環境地図を３次元的に保持しておくことにより、上記領域への移動コストが小さくなり、移動経路となり得る。

なお、階段面ＰＬ_１へ移動可能であると判断する場合には、この階段面ＰＬ_１の床面ＰＬ_０からの高さｈ_１を例えば障害物認識部４０２から受け取り、更に階段面ＰＬ_１がロボット装置１が登ることができる広さを有しているか否かを環境地図５１２により認識し、これらの情報から、階段面ＰＬ_１へ移動可能であるか否かを判断するものとする。また、ここでは、床面ＰＬ_０より高い平面である階段面ＰＬ_１、ＰＬ_２のみを抽出しているが、基準平面ＰＬより低い平面を抽出した場合には、その段差と、登る動作及び降りる動作に基づいたコスト計算がなされるものとする。また、平面内を移動するコストと、平面間を移動するコストとは異なる値を設定しておくことができる。なお、他の平面における環境地図においても、障害物領域Ａ_Ｏである場合は、環境地図を１枚のみ保持している場合と同様、高いコストを設定し、移動不可能と判断させるようにするものとする。

そして、ロボット装置１が階段面ＰＬ_１へ移動可能と判断し、且つ階段面ＰＬ_１を移動経路として選択した場合、この階段面ＰＬ_１へ移動する。ロボット装置１が階段を登るとロボット装置１の脚部が接地している基準平面が階段面ＰＬ_１に移動する。ここで、ロボット装置１は、再び、基準平面ＰＬを抽出し、これに高さ方向が近い２つの平面を抽出する。

基準平面ＰＬ＝階段面ＰＬ_１の場合には、抽出する平面が階段面ＰＬ_１と、床面ＰＬ_０及び階段面ＰＬ_２となり、床面ＰＬ_０にて作成した環境地図をそのまま使用することができる。この際、新しく認識された障害物情報等がある場合は、各環境地図５１１〜５１３を更新するものとする。

以上の処理を繰り返すことで、ロボット装置１は、階段面ＰＬ_１から順次階段を登り階段面ＰＬ_４へ到達することができる。ロボット装置１が移動可能として選択する基準平面に高さが近い平面（階段面）を、本例の場合は２つの平面を選択するものとしたが、ロボット装置１が階段を登る毎又は降りる毎に変化する。即ち、基準平面が変化する毎に抽出する他の平面も変化する。以下、このことを平面のトラッキングともいう。

こうして階段面ＰＬ_３上に登ったロボット装置１の環境地図更新部４０３には、図２０に示すように、床面ＰＬ_０〜階段面ＰＬ_４までの環境地図群５１０が生成されることになる。なお、本例においては、基準平面と、基準平面との高さの差が小さい他の２平面の合計３平面を抽出し、環境地図を構築していくものとしたが、ロボット装置１の移動可能な高さ以内の平面が２以上観測された場合には、これを抽出し、環境地図を構築しておいてもよい。

このように、３次元的に環境地図を保持することにより、平面内のみならず、平面間のコストを算出することができ、これらの値を使用して目的位置までの最小コスト経路を選択することができる。

次に、ロボット装置１が平面間を移動する他の例について説明する。図２２（ａ）及び図２２（ｂ）は、ロボット装置が平面間を移動する他の例を示す模式図であって、図２２（ａ）は、段差を有する異なる部屋を移動するロボット装置を示す図、図２２（ｂ）は、各部屋の環境地図を示す図である。この例においては、部屋Ｒ_Ａは部屋Ｒ_Ｂに隣接しているものとし、部屋Ｒ_Ｂは部屋Ｒ_Ａの床面ＰＬ_Ａより高さＨだけ高い床面ＰＬ_Ｂを有するものとする。

ロボット装置１は、部屋Ｒ_Ａを移動している場合、現在の基準平面は、部屋Ｒ_Ａの床面ＰＬ_Ａとなり、図２２（ｂ）に示すように、その環境地図５２１を有する。ここで、部屋Ｒ_Ｂの床面ＰＬ_Ｂは、部屋Ｒ_Ａの床面ＰＬ_Ａより高いため、障害物領域Ａ_Ｏとされている。そして、部屋Ｒ_Ｂがロボット装置１の観測範囲内に移動すると、部屋Ｒ_Ｂの床面ＰＬ_Ｂは、部屋Ｒ_Ａの床面ＰＬ_Ａと平行な平面であるため抽出され環境地図５２２が作成される。

この部屋Ｒ_Ｂの床面ＰＬ_Ｂの環境地図５２２が作成されると、床面ＰＬ_Ａの環境地図５２１において障害物領域Ａ_Ｏとされ、非常に大きいコストが付されていた床面ＰＬ_Ｂが、環境地図５２２においては、移動可能な自由空間領域Ａ_Ｆとされ、コストが小さくなり、部屋Ｒ_Ｂへ移動するという新たな経路が生まれる。

このように、環境地図５２１の１枚のみでは、ロボット装置１は、部屋Ｒ_Ａ内のみしか移動できなかったのに対し、部屋Ｒ_Ａの床面ＰＬ_Ａと平行な平面の床面ＰＬ_Ｂを自動的に認識して環境地図５２２を作成することで、床面ＰＬ_Ａから床面ＰＬ_Ｂへの平面間の自律的な移動を可能とし、ロボット装置１の移動可能な範囲を拡大することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態におけるロボット装置の概略を示すブロック図である 同ロボット装置のソフトウェアの構成を示す模式図である。 同ロボット装置のソフトウェアの動作を示すフローチャートである。 （ａ）及び（ｂ）は、それぞれ障害物グリッドの算出方法及び絶対座標の算出方法を示すフローチャートである。 同ロボット装置における複数平面抽出から経路計画を行う処理を機能的に分類した図である。 同ロボット装置が平面抽出する際に使用する視差画像の生成を説明するための図である。 同ロボット装置が入力画像から障害物を認識する方法を示すフローチャートである。 同ロボット装置が平面パラメータを検出する方法を示すフローチャートである。 同ロボット装置が平面パラメータを検出する際のパラメータ空間（θ，φ）（投票空間）のヒストグラムを示すグラフ図である。 （ａ）は、複数平面検出部が検出する平面パラメータ（θ，Φ）によるヒストグラムを示す模式図、（ｂ）は、検出された平面パラメータが示す複数平面を模式的に示す斜視図である。 同ロボット装置におけるカメラ座標系から足底接地平面座標系への変換処理を説明するための図である。 検出された平面上の測定点を示す図である。 （ａ）は、同ロボット装置からのロボットビューを示す模式図、（ｂ）は、床面抽出画像を示す模式図、（ｃ）は、（ｂ）を座標変換して平面（２次元）上に障害物かを表現した模式図である。 障害物情報により生成された環境地図上でロボット装置の現在位置から目的位置までの経路を示す模式図である。 （ａ）は、同ロボット装置が階段を登る様子を示す模式図、（ｂ）はその際生成される環境地図を示す図である。 同ロボット装置における経路計画アルゴリズムを示すフローチャートである。 同ロボット装置において実際に観測された環境地図の一例を示す図である。 同じく、同ロボット装置において実際に観測された環境地図の一例を示す図である。 同じく、同ロボット装置において実際に観測された環境地図の一例を示す図である。 階段を登る動作を行うロボット装置を示す模式図である。 図２０に示す階段を登る動作を行うロボット装置が生成する環境地図を示す図である。 （ａ）及び（ｂ）は、ロボット装置が平面間を移動する他の例を示す模式図であって、（ａ）は、段差を有する異なる部屋を移動するロボット装置を示す図、（ｂ）は、各部屋の環境地図を示す図である。 本発明の実施の形態におけるロボット装置の概観を示す斜視図である。 同ロボット装置が具備する関節自由度構成を模式的に示す図である。 同ロボット装置の制御システム構成を示す模式図である。

符号の説明

１ ロボット装置、４０１ 複数平面検出部、４０２ 障害物認識部、４０３ 環境地図更新部、４０４ 経路計画部、 Ａ_Ｏ 障害物領域、Ａ_Ｆ 自由空間領域、Ａ_Ｕ 未観測領域、ＰＬ,ＰＬ_０〜ＰＬ_４ 平面

Claims (24)

1. 障害物が存在する環境下において移動型のロボット装置が環境を認識するための環境認識装置において、
   入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出手段と、
   上記平面検出手段により検出された平面上の障害物を認識する障害物認識手段と、
   上記障害物認識手段により認識された障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新手段とを有し、
   上記平面検出手段は、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、
   上記環境地図更新手段は、上記平面群のうち、上記障害物認識手段により障害物が認識された平面の環境地図を作成及び／又は更新する
   ことを特徴とする環境認識装置。
2. 上記基準平面は、床面である
   ことを特徴とする請求項１記載の環境認識装置。
3. 上記基準平面は、上記ロボット装置が移動している移動面である
   ことを特徴とする請求項１記載の環境認識装置。
4. 上記各平面は、上記ロボット装置が移動可能な面積を有する
   ことを特徴とする請求項１記載の環境認識装置。
5. 上記他の平面は、上記基準平面との高さの差が所定の値以下である平面である
   ことを特徴とする請求項１記載の環境認識装置。
6. 上記他の平面は、上記ロボット装置の認識範囲内において上記基準平面と高さの差が最も小さい平面を含む
   ことを特徴とする請求項１記載の環境認識装置。
7. 上記環境地図は、障害物が存在するとされる障害物領域と、移動可能であるとされる自由空間領域と、未観測である未観測領域とを有する
   ことを特徴とする請求項１記載の環境認識装置。
8. 上記平面検出手段は、上記入力画像として入力される視差画像又は距離画像から変換された３次元データ群から３点以上の３次元データをサンプリングし、上記３次元データによって決まる１つの平面を示す平面パラメータを複数算出する平面算出手段と、上記平面算出手段から得られた複数の平面パラメータを投票空間に投票し該投票結果に基づき上記平面群を決定する平面決定手段とを有する
   ことを特徴とする請求項１記載の環境認識装置。
9. 上記ロボット装置に設けた複数の撮像手段から得られた画像データに基づき算出された視差画像と、該複数の撮像手段が配設された位置における上記視差画像に対応する同次変換行列と、を用いて距離画像を生成する距離画像生成手段を有し、
   上記平面検出手段は、上記距離画像に基づき上記平面パラメータを算出する
   ことを特徴とする請求項１記載の環境認識装置。
10. 上記障害物認識手段は、上記平面群のうち、少なくとも上記移動面上の障害物を認識し、
    上記環境地図更新手段は、上記移動面の環境地図を作成及び／又は更新する
    ことを特徴とする請求項２記載の環境認識装置。
11. 障害物が存在する環境下において移動型のロボット装置が環境を認識するための環境認識方法において、
    入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出工程と、
    上記平面検出工程にて検出された平面上の障害物を認識する障害物認識工程と、
    上記障害物認識工程にて認識された障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新工程とを有し、
    上記平面検出工程では、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、
    上記環境地図更新工程では、上記平面群のうち、上記障害物認識工程にて障害物が認識された平面の環境地図を作成及び／又は更新する
    ことを特徴とする環境認識方法。
12. 上記基準平面は、床面である
    ことを特徴とする請求項１１記載の環境認識方法。
13. 上記基準平面は、上記ロボット装置が移動している移動面である
    ことを特徴とする請求項１１記載の環境認識方法。
14. 上記各平面は、上記ロボット装置が移動可能な面積を有する
    ことを特徴とする請求項１１記載の環境認識方法。
15. 上記他の平面は、上記基準平面との高さの差が所定の値以下である平面である
    ことを特徴とする請求項１０記載の環境認識方法。
16. 上記他の平面は、上記ロボット装置の認識範囲内において上記基準平面と高さの差が最も小さい平面を含む
    ことを特徴とする請求項１１記載の環境認識方法。
17. 障害物が存在する環境下において移動型のロボット装置が現在位置から目的位置まで移動するための経路を計画する経路計画装置において、
    入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出手段と、
    上記平面検出手段により検出された平面群における各平面上の障害物を認識する障害物認識手段と、
    上記障害物認識手段により認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新手段と、
    上記環境地図更新手段により作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画手段とを有する
    ことを特徴とする経路計画装置。
18. 上記経路計画手段は、各環境地図を所定の領域に区切り、上記ロボット装置の現在位置から移動可能とされる各領域毎に該各領域に移動する際のコストを算出し、コスト算出結果に基づき移動経路を計画する
    ことを特徴とする請求項１７記載の経路計画装置。
19. 上記環境地図は、障害物が存在するとされる障害物領域と、移動可能であるとされる自由空間領域と、未観測である未観測領域とを有し、
    上記経路計画手段は、現在移動中の平面に対応する環境地図である第１の環境地図上に上記障害物領域が含まれる場合、上記現在移動中の平面とは異なる平面に対応する環境地図である第２の環境地図を参照し、当該障害物領域が該第２の環境地図上で自由空間領域である場合は、当該障害物領域を移動可能領域とみなす
    ことを特徴とする請求項１７記載の経路計画装置。
20. 障害物が存在する環境下において移動型のロボット装置が現在位置から目的位置まで移動するための経路を計画する経路計画方法において、
    入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出工程と、
    上記平面検出工程にて検出された平面群における各平面上の障害物を認識する障害物認識工程と、
    上記障害物認識工程にて認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新工程と、
    上記環境地図更新工程にて作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画工程とを有する
    ことを特徴とする経路計画方法。
21. 上記経路計画工程では、各環境地図を所定の領域に区切り、上記ロボット装置の現在位置から移動可能とされる各領域毎に該各領域に移動する際のコストを算出し、コスト算出結果に基づき移動経路を計画する
    ことを特徴とする請求項２０記載の経路計画方法。
22. 上記環境地図は、障害物が存在するとされる障害物領域と、移動可能であるとされる自由空間領域と、未観測である未観測領域とを有し、
    上記経路計画工程では、現在移動中の平面に対応する環境地図である第１の環境地図上に上記障害物領域が含まれる場合、上記現在移動中の平面とは異なる平面に対応する環境地図である第２の環境地図を参照し、当該障害物領域が該第２の環境地図上で自由空間領域である場合は、当該障害物領域を移動可能領域とみなす
    ことを特徴とする請求項２０記載の経路計画方法。
23. 障害物が存在する環境下で自律的に移動可能なロボット装置において、
    上記環境を認識するための環境認識装置を有し、
    上記環境認識装置は、
    入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき平面を検出する平面検出手段と、
    上記平面検出手段により検出された平面上の障害物を認識する障害物認識手段と、
    上記障害物認識手段からの障害物情報に基づき環境地図を作成及び／又は更新する環境地図更新手段とを有し、
    上記平面検出手段は、基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出し、
    上記環境地図更新手段は、上記平面群のうち、上記障害物認識手段により障害物が認識された平面の環境地図を作成及び／又は更新する
    ことを特徴とするロボット装置。
24. 障害物が存在する環境下にて現在位置から目的位置まで移動するための経路を自律的に計画して移動するロボット装置において、
    上記環境を認識して移動経路を計画する経路計画装置を有し、
    上記経路計画装置は、
    入力画像に基づき平面パラメータを算出し、該平面パラメータに基づき基準となる基準平面と該基準平面と略平行な１以上の他の平面からなる平面群を検出する平面検出手段と、
    上記平面検出手段により検出された平面群における各平面上の障害物を認識する障害物認識手段と、
    上記障害物認識手段により認識された障害物情報に基づき上記平面群の各平面に対応する環境地図群を作成及び／又は更新する環境地図更新手段と、
    上記環境地図更新手段により作成及び／又は更新された上記環境地図群に基づき上記ロボット装置の移動経路を計画する経路計画手段とを有する
    ことを特徴とするロボット装置。
    

Images