JP2006239844A - 障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置 - Google Patents

Abstract

【課題】ロボット装置を正確にモデル化するとともに高精度に経路を計画し、障害物を回避することができる障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置を提供する。
【解決手段】ロボット装置が移動する基準面からロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する。そして、障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する。
【選択図】図３

Description

本発明は、環境内の情報を取得して作成された障害物環境地図に基づいて経路を計画し、障害物を回避する障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置に関する。

自律型ロボット装置は、産業用ロボットと異なり、周囲の外部状態やロボット自身の内部状態に応じて自律的に動作を行うことができる。例えば、ロボット装置が外部の障害物を検出して障害物を回避するような行動経路を計画することにより自律的に移動することができる。

このような経路計画の研究は、巡回路のなかで最短経路になる巡回順を求める巡回セールスマン問題や干渉チェックにおける計算幾何学の課題とともに人工知能の代表的な研究対象として古くから行われている。この経路計画の代表的な設問として、例えば、複雑に入り組んだ狭い部屋から大きなピアノを出すときのピアノの取るべき経路を幾何学的に求める際の問題、いわゆるPiano Mover's Problemに代表される既知環境における浮遊物体の経路計画が挙げられる。このような計算幾何学と探索アルゴリズムの効率性のベンチマーク的な課題に対して、多くの技術が提案されている。

例えば、特許文献１では、二足歩行ロボット装置に対する障害物回避の経路計画を行っている。これは、計画対象であるロボット装置を円柱形でモデル化し、障害物の状況を表すグリッド（Occupancy Grid）を障害物環境地図として利用することにより、障害物と干渉しないような経路を計画している。

例えば、ロボット装置が現在の位置Ｒからゴール地点Ｇへ辿りつくまでの経路を計画する際、先ず、図２０（ａ）に示すような障害物環境地図を作成する。この障害物環境地図は、ロボット装置が移動する床面からロボット装置がモデル化された円柱３０１の高さまでの間の障害物、又は床面に空いた穴などを示すものである。ここで、黒色のセルは障害物が占有している領域を示し、白色のセルはロボット装置が自由に移動できる自由領域を示している。そして、この図２０（ａ）に示す環境地図中の障害物の占有領域を円柱３０１の半径分だけ拡大させることにより、ロボット装置は図２０（ｂ）に示すように現在の位置Ｒに収縮したのと等価となる。この拡大させた障害物領域との重なりをチェックすることで、ロボット装置と障害物との干渉チェックを行うことができる。そして、例えば、経路全体のコストを最小化するＡ^＊探索と呼ばれる方法を用いることにより、図２０（ｂ）に示す矢印に示すような障害物を回避した経路を計画することができる。

特開２００３−２６６３４５号公報

上述のようにロボット装置を単一の円柱でモデル化した場合、ロボット装置全体を含むように半径が大きな円柱でモデル化し、障害物環境地図中の障害物の占有領域をその円柱の半径で拡大するため、モデル誤差を生じる。

例えば、図２１に示すようなロボット装置の胴体部よりも低くロボット装置の腕部が通過可能な部分Ａを有する障害物３０２が環境内に存在する場合、図２２（ａ）に示すような障害物環境地図が得られるが、斜線で示す低い部分Ａを有する障害物３０２を、モデル化された円柱３０３の半径で拡大した場合、図２２（ｂ）に示すようにロボット装置の胴体部よりも低い部分Ａも障害物として認識されてしまう。したがって、現在のロボット装置の位置Ｒからゴール地点Ｇまでの経路計画をすることができなかった。

本発明は、このような課題を解決するために提案されたものであり、ロボット装置を正確にモデル化するとともに高精度に経路を計画し、障害物を回避することができる障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置を提供することを目的とする。

上記課題を解決するために、本発明に係る障害物回避装置は、移動型ロボット装置が障害物を回避するための障害物回避装置において、上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成手段と、上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画手段とを備えることを特徴としている。

また、本発明に係る障害物回避方法は、移動型ロボット装置が障害物を回避するための障害物回避方法において、上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成工程と、上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画工程とを有することを特徴としている。

また、本発明に係る障害物回避プログラムは、移動型ロボット装置が障害物を回避するための障害物回避プログラムにおいて、上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成工程と、上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画工程とを有することを特徴としている。

また、本発明に係る移動型ロボット装置は、障害物を回避する移動型ロボット装置において、自身が移動する基準面から自身の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成手段と、上記障害物環境地図の障害物の占有領域を各レイヤの自身の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画手段とを備えることを特徴としている。

ロボット装置が移動する基準面からロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成し、これら障害物環境地図の障害物の占有領域を各レイヤのロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画することにより、高精度且つ効率的に障害物を回避することができる。

以下、図面を参照しながら本発明の実施の形態について説明する。この実施の形態は、本発明に係る障害物回避装置を２足歩行のロボット装置に適用したものである。

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

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

本実施の形態のロボット装置１におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置１の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置１が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。

図２は、ロボット装置１に搭載される障害物回避装置２の機能ブロック図である。この障害物回避装置２は、障害物回避プログラムを基にＣＰＵ１５内にて構成される。

障害物回避装置２は、３次元座標変換部（Coordinate Transformation）３と、障害物環境地図更新部（Update layered Occupancy Grid）４と、ロボット座標変換部（Kinematics）５と、ロボット座標更新部（Update Robot pose）６と、経路計画部（Path Planner）７とを備えて構成されている。

３次元座標変換部３は、ＣＣＤカメラ１０Ｒ，１０Ｌにより入力されたステレオデータに基づいてロボット装置１の基準座標に変換する。具体的には、平面検出器が人間の両眼に相当する左右２つのカメラからの画像入力を画素近傍毎に比較し、その視差から対象までの距離を推定し、平面を検出することによって環境内に存在する複数の平面を認識して変換し、３次元の位置情報を出力する。

障害物環境地図更新部４は、３次元位置情報に基づいて障害物環境地図の作成及び更新を行う。障害物環境地図の作成は、図３に示すようにロボット装置１を円柱の合成としてモデル化し、円柱毎に障害物環境地図であるグリッドを作成する。このグリッドはモデル化された円柱の底面から上面までの高さの範囲に障害物が存在するか否かを示している。

例えば、図３に示すようにロボット装置１は、高さ位置が０からｈ_ｍａｘまでの円柱１と、高さ位置がｈ_ｌｏｗからｈ_ｍａｘまでの円柱２とによりモデル化される。この円柱モデルは、床面からロボット装置１の高さまでの高さ範囲を有するとともに可動領域を含む脚部の断面を包含する半径を有する円柱１と、胴部及び頭部を含む高さ範囲を有すとともに稼動領域を含む腕部を包含する円柱１よりも大きい半径を有する円柱２とを用いて構成され、各円柱の中心軸は一致するようにモデル化されている。円柱１に対応するグリッド１は、０からｈ_ｍａｘまでの高さ位置における障害物環境を示し、円柱２に対応するグリッド１は、ｈ_ｌｏｗからｈ_ｍａｘまでの高さ位置における障害物環境を示す。また、後述するが、グリッド１は、高さ位置が０以下、すなわちロボット装置が移動する床面以下の環境状態も示すことができる。

グリッドの各セルには、物体によって占有されているか否かの確率が付与され、観測データに基づいて各セルの占有確率が変更される。そして、占有確率に閾値を設け、占有確率が閾値より大きいか否かによって環境の状態を表現する。

また、障害物環境地図更新部４は、測定点ｐが床面付近の場合、測定点ｐのｘ座標及びｙ座標に対応するセルにエンプティ（Empty）処理を行う。また、測定点ｐが床面付近でない場合、測定点ｐのｘ座標及びｙ座標に対応するセルに対してオキュパイド（Occupied）処理を行う。

このグリッドは、セルＣに対して障害物の存在確率（障害物によって占有されている確率）ｐ（ｃ）を保持しており、このＥＭＰＴＹ処理及びＯＣＣＵＰＩＥＤ処理は、これら各セルに対して統計的に行う。ＥＭＰＴＹ処理は、障害物の存在確率を低下させる処理であり、ＯＣＣＵＰＩＥＤ処理は障害物の存在確率を増加させる処理である。本具体例では、ＥＭＰＴＹ処理及びＯＣＣＵＰＩＥＤ処理の存在確率を算出する手法の一例として、ベイズの更新則を用いる。

ＥＭＰＴＹ処理のとき式（１）により占有確率を減少させ、ＯＣＣＵＰＩＥＤ処理のとき式（２）により占有確率を増加させる。式（１）は、セルＣの占有確率を表すｐ（ｃ）が“占有”という条件下にあるセルＣの確率を表しており、式（１）又は式（２）においてセルＣが占有されているか又は占有されていないかを表す確率ｐ（occ｜．．．）、ｐ（empty｜．．．）は、予め決められた閾値ｔｈである。

このように障害物の存在確率は、セルごとに算出され、障害物環境地図が更新される。

ロボット座標変換部５は、ロボット装置１の内部の関節角情報（Joint Angle）に基づいて移動量（Odometry）を導出する。

ロボット座標更新部６は、移動量に基づいてロボット装置１の位置及び姿勢を更新する。具体的には移動量を時間方向に渡って積算することにより更新する。

経路計画部７は、障害物環境地図に基づいて経路を計画する。経路計画には、例えば、経路全体のコストを最小化するＡ^＊探索と呼ばれる方法が用いられる。

上述した障害物回避装置２の適用例として、図４に示すような障害物が存在する場合について説明する。障害物Ａは図３に示すようにモデル化された円柱２の高さ位置ｈ_ｌｏｗよりも高さが低い障害物である。また、障害物Ｂは図３に示す円柱２の高さ位置ｈ_ｌｏｗよりも高さが低い部分を有するものである。また、障害物Ｃは三角形状であり、図３に示す円柱２の高さ位置ｈ_ｌｏｗよりも高さが低い部分を有している。

図５は、図４に示すような障害物とグリッドとの関係を示す模式図である。図３に示す２つの円柱によるロボット装置１のモデル化の例では、図５に示すように円柱の高さ範囲に対応して異なる２つのレイヤに分けられる。レイヤ１における障害物はグリッド１に表示され、レイヤ２における障害物はグリッド２に表示される。

グリッド１及びグリッド２において、白色のセルはロボット装置１が自由に移動できる自由領域、黒色のセルは障害物及び灰色のセルは未観測領域を示している。ここで、測定点がレイヤ２に存在する場合、レイヤ１にも存在することになるので、グリッド１及びグリッド２が更新される。また、測定点がレイヤ１のみに存在する場合、つまり測定点がレイヤ２よりも低い位置に存在する場合、グリッド１が更新される。

図６は、障害物環境地図更新部４におけるグリッドの更新処理を説明するフローチャートである。障害物環境地図更新部４は、３次元の位置情報の各データ点Ｐ＝（ｘ，ｙ，ｚ）に対して以下の処理を行う。先ず、点Ｐの（ｘ，ｙ）座標から点Ｐが属するグリッドのセル（ｉ，ｊ）を求める（ステップＳ１１）。次に、点Ｐの高さｚをロボット装置１が直立している床面の高さ（h_floor）と比較することで点Ｐが床面付近のデータであるかどうかを判断する（ステップＳ１２）。

ステップＳ１２において、点Ｐが床面付近のデータである場合には、点Ｐが属するグリッド１とグリッド２のセル（ｉ，ｊ）を障害物が存在しないことを示す値ＥＭＰＴＹで更新し（ステップＳ１３及びステップＳ１４）、処理を終了する。

また、ステップＳ１２において、点Ｐが床面付近のデータでない場合には、ステップＳ１５に進み、点Ｐの高さｚとレイヤ２の下端の高さ位置ｈ_ｌｏｗとを比較する。

ステップＳ１５において、点Ｐの高さｚがｈ_ｌｏｗより低い場合には、グリッド１のセル（ｉ，ｊ）を障害物であることを示す値ＯＣＣで更新する（ステップＳ１６）。点Ｐの高さｚが床面付近ではなく、点Ｐの高さｚがレイヤ２の下端の高さ位置ｈ_ｌｏｗより低い場合、点Ｐは床面高さからｈ_ｌｏｗまでの間に存在するデータ又は床面よりも低いデータである。このように、グリッド１には床面に空いた穴（ｚ＜h_floor）も障害物として示すことができる。

また、ステップＳ１５において、点Ｐの高さｚがｈ_ｌｏｗ以上の場合、レイヤ１及びレイヤ２の上端の高さ位置ｈ_ｍａｘと比較する（ステップＳ１７）。

ステップＳ１７において、点Ｐの高さｚがｈ_ｍａｘより低い点の場合、グリッド１及びグリッド２のセル（ｉ，ｊ）を障害物であることを示す値ＯＣＣで更新し（ステップＳ１６及びステップＳ１８）、処理を終了する。

また、ステップＳ１７において、点Ｐの高さｚがｈ_ｍａｘ以上の場合、処理を終了する。

このような処理によって、ロボット装置１が観測した環境内の障害物を、異なる２つのレイヤのグリッドとして表現することができる。また、このグリッドは、セルに対する障害物の存在確率ｐ（ｃ）によって表現され、セルの更新の際は、上書き更新ではなく、統計的な更新をするため、誤認識などのノイズに対してロバストな障害物環境地図構築が可能となる。

続いて、上述した処理によって更新されたグリッド１及びグリッド２の干渉チェックについて説明する。図７はレイヤ１のグリッド１を示す一例であり、図８はレイヤ２のグリッド２を示す一例である。ここで、黒色のセルは障害物が占有している領域を示し、白色のセルはロボット装置１が自由に移動できる自由領域を示している。また、灰色のセルは、後述するが、障害物をモデル化された円柱の半径分拡大させた領域である。

ここで、図７（ａ）に示すグリッド１と図８（ａ）に示すグリッド２とを比較すれば分かるように、グリッド１とグリッド２とは異なるものとなっている。これは、グリッド１が上述したように高さ位置０からｈ_ｍａｘまでに存在する障害物の情報を有し、グリッド２が高さ位置ｈ_ｌｏｗからｈ_ｍａｘまでに存在する障害物の情報を有しているためである。つまり、図７に示すグリッド１中の斜線で示すセルは、高さ位置０からｈ_ｌｏｗまでに存在する障害物である。

図７（ａ）に示すグリッド１において、障害物を示すセルをモデル化された円柱の円Ｒ１の半径分拡大すると、図７（ｂ）に示すようにロボット装置１は点Ｒに収束するとともに、円柱の半径分拡大させた灰色のセルは障害物として認識されるようになる。

また、図８（ａ）に示すグリッド２も同様に、障害物を示すセルをモデル化された円柱の円Ｒ２の半径分拡大すると、図８（ｂ）に示すようにロボット装置１は点Ｒに収束するとともに、円柱の半径分拡大させた灰色のセルは障害物として認識されるようになる。

そして、図７（ｂ）及び図８（ｂ）に示すグリッドを合成することにより、図９に示すような環境地図を得ることができる。この環境地図は、図２２（ｂ）に示す従来の単一の円柱モデルによる障害物環境地図と比較すれば分かるように、図２２（ｂ）に示す障害物環境地図では障害物として認識され、ロボット装置１の位置ＲからゴールＧまでの経路を計画できなかった領域が、障害物として認識されていない。このため、図９に示す環境地図によれば、位置ＲからゴールＧまでの経路計画が可能となる。

経路計画は、経路計画部７によって図９に示す環境地図に基づいて行われる。経路計画には、例えば、経路全体のコストを最小化するＡ^＊探索と呼ばれる方法が用いられる。具体的には、スタートセルから始めて、式（３）に示す評価関数ｆの評価値（コスト）が最小となるセルを順次展開することにより経路を探索する。

また、g(n)は現在のノードでのコスト、h(n,goal)は現在のノードからゴールまでの経路の見積もりコスト、及びαは重み係数である。

このようにロボット装置１を２つの円柱でモデル化し、各円柱の高さ範囲に対応するレイヤのグリッドを作成することにより、高さや半径が異なる円柱ごとに干渉チェックを行うことができる。これにより、干渉チェックが高精度となり、ロボット装置１が移動可能な領域が増加する。

なお、上記実施の形態では、ロボット装置１を２つの円柱でモデル化することとしたが、これに限られるものではなく、複数の円柱でモデル化するようにしてもよい。複数の円柱でモデル化した場合、環境空間が複数のレイヤに分けれ、複数のグリッドが生成されることになる。この場合の障害物環境地図更新部４における複数のグリッドの更新処理を図１０に示すフローチャートを参照して説明する。この例では、ｎ個の円柱（ｋ＝１，２…ｎ）によってロボット装置１がモデル化されており、対応するｎ個のグリッド（ｋ＝１，２…ｎ）が存在するものとする。また、ロボット装置１が直立している床面より下の情報をグリッド０によって表現している。

障害物環境地図更新部４は、３次元の位置情報の各データ点Ｐ＝（ｘ，ｙ，ｚ）に対して以下の処理を行う。先ず、点Ｐの（ｘ，ｙ）座標から点Ｐが属する障害物環境グリッドのセル（ｉ，ｊ）を求める（ステップＳ２１）。次に、点Ｐの高さｚをロボット装置１が直立している床面の高さ（h_floor）と比較することで点Ｐが床面付近のデータであるかどうかを判断する（ステップＳ２２）。

ステップＳ２２において、点Ｐが床面付近のデータである場合には、すべてのグリッドｋ（ｋ＝１，２…ｎ）のセル（ｉ，ｊ）を障害物が存在しないことを示す値ＥＭＰＴＹで更新する（ステップＳ２３〜Ｓ２６）。すなわち、ｋ＝１から代入してゆき（ステップＳ２３）、ステップＳ２４においてｋがｎより小さい場合、そのセルをEMPTYで更新し（ステップＳ２３）、ｋをインクリメントする（ステップＳ２６）。そして、ステップＳ２４に戻り、ｋとｎの値が比較される。ステップＳ２４においてｋがｎ以上の場合、更新処理を終了する。

ステップＳ２２において、点Ｐが床面付近のデータでない場合には、点Ｐが床面よりも低い位置のデータ点であるかどうかを判断する（ステップＳ２７）。

ステップＳ２７において、点Ｐの高さｚが床面よりも低い場合、グリッド０のセル（ｉ，ｊ）を障害物であることを示す値ＯＣＣで更新し（ステップＳ２８）、処理を終了する。

ステップＳ２７において、点Ｐの高さが床面以上の場合、高さｚが含まれるレイヤのグリッドｋ（ｋ＝１，２…ｎ）のセル（ｉ，ｊ）をＯＣＣで更新する（ステップＳ２９〜Ｓ３３）。すなわち、ｋ＝１から代入してゆき（ステップＳ２９）、ステップＳ３０においてｋがｎ以下の場合、ステップＳ３１に進み、点Ｐの高さｚがグリッドｋのレイヤの高さ位置ｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれるか否かが判断される（ステップＳ３１）。点Ｐの高さｚがグリッドｋのレイヤの高さ位置ｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれる場合、グリッドｋのセル（ｉ，ｊ）をＯＣＣで更新する（ステップＳ３２）。そして、ｋをインクリメントし（ステップＳ３３）、ステップＳ３０に戻る。

また、ステップＳ３１において点Ｐの高さｚがグリッドｋのレイヤの高さ位置ｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれない場合、ｋをインクリメントし、ステップＳ３０に戻る。

ステップＳ３０においてｋがｎより大きい場合、更新処理を終了する。このように環境が複数のレイヤに分けられたグリッドを更新処理することにより、複雑な環境にも対応することができる。

また、上述した更新処理では、ロボット装置１が床面を観測した際、すべてのレイヤのグリッドセルに対してＥＭＰＴＹ処理を行うため、問題が生じることがある。例えば、図１１に示すような、上部が下部よりも大きい形状を有する障害物Ａや三角形の頂点が床面と接するような三角形状の障害物Ｂの場合、図１２に示すようにロボット装置１が床面を観測した際、上述の更新処理ではすべてのレイヤのグリッドセルに対してＥＭＰＴＹの処理を行う。そのため、たとえ前回、上部の障害物部分を認識していたとしても、図１２に示すようにロボット装置１が床面を観測してＥＭＰＴＹ処理を行うことによって、障害物が障害物環境地図上から消去されてしまう虞がある。

そこで、ロボット装置１からの距離に応じて近傍領域（Near Region）と非近傍領域（Far Region）を定義し、遠くの床面に対してはＥＭＰＴＹの処理を行わず、近傍距離の床面のみにＥＭＰＴＹの処理を行う。この近傍を定義するための距離は、各レイヤ独立に定義する。例えば、図１２に示すように、レイヤ１には近傍領域のｎｅａｒ＿１と非近傍領域のｆａｒ＿１とを定義し、レイヤ２には近傍領域のｎｅａｒ＿２と非近傍領域のｆａｒ＿２とを定義し、床面を含むレイヤ１の近傍領域ｎｅａｒ＿１をレイヤ２の近傍領域ｎｅａｒ＿２よりも大きくする。これによりロボット装置１のＣＣＤカメラ１０Ｒ，１０Ｌの各レイヤの撮像範囲に入らないような上部が下部よりも大きい形状の障害物に対するＥＭＰＴＹ処理を避けることができる。

ロボット装置１からの距離に応じて近傍領域と非近傍領域を定義した場合の障害物環境地図更新部４の処理を図１３に示すフローチャートを参照して説明する。この例では、ｎ個の円柱（ｋ＝１，２…ｎ）によってロボット装置１がモデル化されており、各円柱の高さに対応するｎ個のグリッド（ｋ＝１，２…ｎ）が存在するものとする。また、ロボット装置１が直立している床面より下の情報をグリッド０によって表現している。

障害物環境地図更新部４は、３次元の位置情報の各データ点Ｐ＝（ｘ，ｙ，ｚ）に対して以下の処理を行う。先ず、点Ｐの（ｘ，ｙ）座標から点Ｐが属する障害物環境グリッドのセル（ｉ，ｊ）を求める（ステップＳ４１）。次に、点Ｐの高さｚをロボット装置１が直立している床面の高さ（h_floor）と比較することで点Ｐが床面付近のデータであるかどうかを判断する（ステップＳ４２）。

ステップＳ４２において、点Ｐが床面付近のデータである場合には、すべてのグリッドｋ（ｋ＝１，２…ｎ）に対し、その点Ｐがロボット装置１の近傍か否かの判定を行う（ステップＳ４３〜Ｓ４７）。すなわち、ｋ＝１から代入してゆき（ステップＳ４３）、ステップＳ４４においてｋがｎ以下の場合、グリッドｋにおける点Ｐが近傍領域（near_k）に存在するか否かを判定する（ステップＳ４５）。

ステップＳ４５において、グリッドｋにおける点Ｐが近傍領域の場合、グリッドｋのセルｋ（ｉ，ｊ）をＥＭＰＴＹ処理する（ステップＳ４６）。また、ステップＳ４５において、グリッドｋにおける点Ｐが近傍領域でない場合、ｋをインクリメントし（ステップＳ４７）、ステップＳ４４に戻る。

ステップＳ４４において、ｋがｎより大きい場合、更新処理を終了する。

ステップＳ４２に戻って、点Ｐが床面付近のデータでない場合には、点Ｐが床面より低い位置のデータ点であるかどうかを判断する（ステップＳ４８）。

ステップＳ４８において、点Ｐの高さｚが床面よりも低い場合、グリッド０のセル（ｉ，ｊ）を障害物であることを示す値ＯＣＣで更新し（ステップＳ４９）、処理を終了する。

また、ステップＳ４８において、点Ｐの高さｚが床面以上の場合、高さｚを含むレイヤのグリッドｋ（ｋ＝１，２…ｎ）のセル（ｉ，ｊ）をＯＣＣで更新する（ステップＳ５０〜Ｓ５４）。すなわち、ｋ＝１から代入してゆき（ステップＳ５０）、ステップＳ５１においてｋがｎ以下の場合、ステップＳ５２に進み、点Ｐの高さｚがグリッドｋのレイヤの高さ位置ｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれるか否かが判断される（ステップＳ５２）。点Ｐの高さｚがグリッドｋのｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれる場合、グリッドｋのセル（ｉ，ｊ）をＯＣＣで更新する（ステップＳ５３）。そして、ｋをインクリメントし（ステップＳ５４）、ステップＳ５２に戻る。

また、ステップＳ５２において点Ｐの高さｚがグリッドｋのレイヤの高さ位置ｈ＿ｋ＿ｈｉｇｈ＞ｚ＞ｈ＿ｋ＿ｌｏｗの範囲に含まれない場合、ｋをインクリメントし、ステップＳ５１に戻る。

ステップＳ５１においてｋがｎより大きい場合、更新処理を終了する。

このようにロボット装置１からの距離に応じて近傍領域と非近傍領域を定義し、遠くの床面に対してはＥＭＰＴＹの処理を行わず、近傍距離の床面のみにＥＭＰＴＹの処理を行うことにより、例えば、図１１に示すような上部が下部よりも大きい障害物との衝突を回避することができる。

また、ロボット装置１には様々なモデル形態を用いることができる。例えば、２つの円柱を用いて、図１４（ａ）に示すように、脚部を円柱１４１でモデル化し、頭部と胴体部を円柱１４２でモデル化してもよい。また、図１４（ｂ）に示すように頭部から脚部までを円柱１４３でモデル化し、胴体部を円柱１４４でモデル化してもよい。

また、３つの円柱を用いて、図１５及び図１６に示すようにロボット装置１をモデル化してもよい。図１５（Ａ）の例は、胴体部から脚部までを円柱１５１、胴体部を円柱１５２及び頭部から脚部までを円柱１５３でモデル化している。図１５（Ｂ）の例は、胴体部から脚部までを円柱１５４、胴体部を円柱１５５及び頭部を円柱１５６でモデル化している。図１５（Ｃ）の例は、胴体部から脚部までを円柱１５７、胴体部を円柱１５８及び頭部から胴体部までを円柱１５９でモデル化している。図１６（Ａ）の例は、脚部を円柱１６１、胴体部を円柱１６２及び頭部から脚部までを円柱１６３でモデル化している。図１６（Ｂ）の例は、脚部を円柱１６４、胴体部を円柱１６５及び頭部を円柱１６６でモデル化している。図１６（Ｃ）の例は、脚部を円柱１６７、胴体部を円柱１６８及び頭部から胴体部までを円柱１６９でモデル化している。

このようにロボット装置１を水平面と平行な底面を有するとともに中心軸を共有する円柱を用いてモデル化することにより、障害物情報が異なる様々なグリッドが作成されるため、複雑な外部環境にも対応することができる。

次に、障害物回避装置２が搭載されるロボット装置１について図１７乃至図１９を参照して、さらに詳細に説明する。図１７は、図１に示すロボット装置１の詳細な外観を示す斜視図である。図１７に示すように、ロボット装置１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている。

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

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

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

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

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

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

このようなロボット装置１は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット２０２等に備える。図１９は、ロボット装置１の制御システム構成を示す模式図である。図１９に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール２００と、アクチュエータ３５０の駆動等、ロボット装置１の全身協調運動を制御する運動制御モジュール３００とで構成される。

思考制御モジュール２００は、情緒判断や感情表現に関する演算処理を実行するＣＰＵ（Central Processing Unit）２１１や、ＲＡＭ（Random Access Memory）２１２、ＲＯＭ（Read Only Memory）２１３及び外部記憶装置（ハード・ディスク・ドライブ等）２１４等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。

この思考制御モジュール２００は、画像入力装置２５１から入力される画像データや音声入力装置２５２から入力される音声データ等、外界からの刺激等にしたがって、ロボット装置１の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット装置１の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置２５１は、例えばＣＣＤ（Charge Coupled Device）カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。また、音声入力装置２５２は、例えばマイクロホンを複数備えている。

思考制御モジュール２００は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール３００に対して指令を発行する。

一方の運動制御モジュール３００は、ロボット装置１の全身協調運動を制御するＣＰＵ３１１や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。

この運動制御モジュール３００には、図１９に示したロボット装置１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インターフェース（Ｉ／Ｆ）３１０経由で接続されている。ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組合せによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。

思考制御モジュール２００と運動制御モジュール３００は、共通のプラットフォーム上で構築され、両者間はバス・インターフェース２１０，３１０を介して相互接続されている。

運動制御モジュール３００では、思考制御モジュール２００から指示された行動を体現すべく、各アクチュエータ３５０による全身協調運動を制御する。すなわち、ＣＰＵ３１１は、思考制御モジュール２００から指示された行動に応じた動作パターンを外部記憶装置３１４から取り出し、又は、内部的に動作パターンを生成する。そして、ＣＰＵ３１１は、指定された動作パターンにしたがって、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各アクチュエータ３５０に転送する。

また、ＣＰＵ３１１は、姿勢センサ３５１の出力信号によりロボット装置１の体幹部ユニット２０２の姿勢や傾きを検出するとともに、各接地確認センサ３５２，３５３の出力信号により各脚部ユニット２０５Ｒ／Ｌが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット装置１の全身協調運動を適応的に制御することができる。更に、ＣＰＵ３１１は、ＺＭＰ位置が常にＺＭＰ安定領域の中心に向かうように、ロボット装置１の姿勢や動作を制御する。

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

本発明の具体例として示すロボット装置の概略を説明するブロック図である。 本実施の形態における障害物回避装置を説明する機能ブロック図である。 上記ロボット装置を円柱でモデル化した一例を示す図である。 障害物の形状を説明するための図である。 円柱に対応したレイヤを説明するための図である。 障害物環境地図を更新する処理例を説明するフローチャートである。 干渉チェックを説明するための図である。 干渉チェックを説明するための図である。 障害物領域が拡大された各環境地図を合成したものを説明するための図である。 障害物環境地図を更新する処理例を説明するフローチャートである。 障害物の形状を説明するための図である。 円柱に対応したレイヤを説明するための図である。 障害物環境地図を更新する処理例を説明するフローチャートである。 ２つの円柱によるモデル例を示す図である。 ３つの円柱によるモデル例を示す図である。 ３つの円柱によるモデル例を示す図である。 本発明の実施の形態におけるロボット装置の外観を示す斜視図である。 上記ロボット装置が具備する関節自由度構成を模式的に示す図である。 上記ロボット装置の制御システム構成を示す模式図である。 干渉チェックを説明するための図である。 単一の円柱モデルにより干渉チェックした場合を説明するための図である。 単一の円柱モデルによる干渉チェック例を示す図である。

符号の説明

１ ロボット装置、 ２ 障害物回避装置、 ３ ３次元座標変換部、 ４ 障害物地図更新部、 ５ ロボット座標変換部、 ６ ロボット座標更新部、 ７ 経路計画部

Claims (9)

1. 移動型ロボット装置が障害物を回避するための障害物回避装置において、
   上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成手段と、
   上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画手段と
   を備えることを特徴とする障害物回避装置。
2. 上記移動型ロボット装置は、上記所定の高さ範囲に対応し、上記基準面に対して鉛直、且つ中心軸を共有する複数の円柱によってモデル化され、
   上記断面形状は、円形であることを特徴とする請求項１記載の障害物回避装置。
3. 上記経路計画手段は、上記拡大環境地図を複数重ね合わせて経路を計画することを特徴とする請求項１記載の障害物回避装置。
4. 上記基準面を含むレイヤの障害物環境地図の少なくとも１つは、上記基準面よりも低い位置を障害物として表すことを特徴とする請求項１記載の障害物回避装置。
5. 上記障害物を含む環境の３次元位置情報を取得する取得手段と、
   上記取得手段にて取得した３次元位置情報の高さ位置に応じて所定のレイヤの障害物環境地図を更新する障害物環境地図更新手段と
   をさらに備えることを特徴とする請求項１記載の障害物回避装置。
6. 上記３次元位置情報を取得するための撮像手段をさらに備え、
   上記障害物環境地図更新手段は、上記撮像手段の各レイヤの撮像範囲に基づいて障害物環境地図内の所定の領域を更新することを特徴とする請求項５記載の障害物回避装置。
7. 移動型ロボット装置が障害物を回避するための障害物回避方法において、
   上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成工程と、
   上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画工程と
   を有することを特徴とする障害物回避方法。
8. 移動型ロボット装置が障害物を回避するための障害物回避プログラムにおいて、
   上記移動型ロボット装置が移動する基準面から上記移動型ロボット装置の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成工程と、
   上記障害物環境地図の障害物の占有領域を各レイヤの上記移動型ロボット装置の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画工程と
   を有することを特徴とする障害物回避プログラム。
9. 障害物を回避する移動型ロボット装置において、
   自身が移動する基準面から自身の高さまでの高さ範囲を、それぞれ所定の高さ範囲に対応した複数のレイヤに分割し、各レイヤに存在する障害物の占有状態を示す障害物環境地図を作成する障害物環境地図作成手段と、
   上記障害物環境地図の障害物の占有領域を各レイヤの自身の断面形状の大きさに応じて拡大させた拡大環境地図に基づいて経路を計画する経路計画手段と
   を備えることを特徴とする移動型ロボット装置。

Images