(57)【要約】【課題】干渉チェック等の計算量を削減するとともに効率的に経路を計画し、障害物を回避することができる障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置を提供する。【解決手段】ロボット装置の方向毎に層を設け、各層で共通の地図を有する状態空間を構成し、ロボット装置の行動に関する評価値に基づいて上記状態空間内に経路を生成する。そして、生成された経路に基づいて移動体の行動を制御し、障害物を回避する。【選択図】図５
2006205348.tif 000002
【特許請求の範囲】
【請求項１】
移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避装置において、
上記地図における移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成手段と、
上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索手段と、
上記経路に基づいて上記移動体の動作を制御する経路追跡手段と
を備えることを特徴とする障害物回避装置。
【請求項２】
上記移動体の動作を離散的に構成する複数の歩行の基本的な要素と、各要素によって遷移する移動量と、各要素の遷移に対する評価値とが蓄積されたデータベースをさらに備え、
上記経路探索手段は、上記歩行の基本的な要素を遷移させながら経路を生成することを特徴とする請求項１記載の障害物回避装置。
【請求項３】
上記状態空間構成手段は、上記状態空間の地図と移動体の方向とを上記データベースに基づいて構成することを特徴とする請求項２記載の障害物回避装置。
【請求項４】
上記状態空間は、上記移動体を４５度毎に回転させた８方向の層で構成されていることを特徴とする請求項１記載の障害物回避装置。
【請求項５】
上記評価値は、上記移動体の動作に要する時間、動作の安全性、動作の変更に対するペナルティ、動作に関する制限のうち、少なくとも１以上に基づいて定義されることを特徴とする請求項１記載の障害物回避装置。
【請求項６】
移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避方法において、
上記地図における上記移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成工程と、
上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索工程と、
上記経路に基づいて上記移動体の動作を制御する経路追跡工程と
を有することを特徴とする障害物回避方法。
【請求項７】
移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避プログラムにおいて、
上記地図における上記移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成工程と、
上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索工程と、
上記経路に基づいて上記移動体の動作を制御する経路追跡工程と
を備えることを特徴とする障害物回避プログラム。
【請求項８】
移動可能な領域が判別された地図に基づいて障害物を回避する移動型ロボット装置において、
上記地図における自身の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成手段と、
自身の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索手段と、
上記経路に基づいて自身の動作を制御する経路追跡手段と
を備えることを特徴とする移動型ロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、環境内の情報を取得して作成された環境地図に基づいて経路を計画し、障害物を回避する障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置に関する。
【背景技術】
【０００２】
自律型ロボット装置は、産業用ロボットと異なり、周囲の外部状態やロボット自身の内部状態に応じて自律的に動作を行うことのできるロボット装置である。例えば、歩行可能なロボット装置では、外部の障害物を検出して障害物を回避するような行動経路を計画することにより自律的に移動する。
【０００３】
このような経路計画の研究は、巡回路のなかで最短経路になる巡回順を求める巡回セールスマン問題や干渉チェックにおける計算幾何学の課題とともに人工知能の代表的な研究対象として古くから行われている。この経路計画の代表的な設問として、例えば、複雑に入り組んだ狭い部屋から大きなピアノを出すときのピアノの取るべき経路を幾何学的に求める際の問題、いわゆるPiano Mover's Problemに代表される既知環境における浮遊物体の経路計画が挙げられる。このような計算幾何学と探索アルゴリズムの効率性のベンチマーク的な課題に対して、多くの技術が提案されている。
【０００４】
例えば、特許文献１では、二足歩行ロボット装置に対する障害物回避の経路計画を行っている。これは、計画対象であるロボット装置を円柱形でモデル化し、障害物の状況を表すグリッド（Occupancy Grid）を環境地図として利用することにより、障害物と干渉しないような経路を計画している。この技術によれば、ロボット装置が円柱表現されているため、干渉チェックを非常に高速に行うことができる。
【０００５】
また、非特許文献１では、ロボット装置のモデルとして図２９に示すような矩形モデルを利用し、図３０に示す代表的な姿勢ごとに離散的な複数のレイヤを有する空間を提案している。これにより、連続空間に比べて高速な経路計画を実現し、図３１に示すように姿勢を変化させた移動を可能としている。
【０００６】
【特許文献１】特開２００３−２６６３４５号公報
【非特許文献１】Z. Shiller, K. Yamane, Y. Nakamura. “Planning Motion Patterns of Human Figures Using a Multi-layered Grid and the Dynamics Filter”, Int. Conf. on Robotics and Automation （ICRA）, 2001, Multi-layered 2D grid, each layer representing a posture of the agent.
【発明の開示】
【発明が解決しようとする課題】
【０００７】
特許文献１に記載された技術は、計算量削減のためロボット装置の姿勢を考慮していない、２次元の経路計画である。また、全方位移動可能であるホロノミックなロボット装置であると仮定して経路計画を行っている。しかし、実際の二足歩行ロボット装置は、その場における旋回などのホロノミックな行動をすることができない。したがって、計画された経路が真に安全であるためには、どのような行動をしても障害物に衝突しないように、ロボット装置のモデルである円柱の半径を大きく設定しなければならなかった。
【０００８】
また、非特許文献１では、各レイヤにおいてロボット装置の矩形モデルが異なるため、高負荷な干渉チェックをモデルごとに独立に行う必要がある。また、姿勢変化に伴う各モデル間の遷移に関して特別な干渉チェックが必要であるため、計算量が膨大となっていた。
【０００９】
本発明は、このような課題を解決するために提案されたものであり、干渉チェック等の計算量を削減するとともに効率的に経路を計画し、障害物を回避することができる障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置を提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記課題を解決するために、本発明に係る障害物回避装置は、移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避装置において、上記地図における上記移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成手段と、上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索手段と、上記経路に基づいて上記移動体の動作を制御する経路追跡手段とを備えることを特徴としている。
【００１１】
また、本発明に係る障害物回避方法は、移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避方法において、上記地図における上記移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成工程と、上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索工程と、上記経路に基づいて上記移動体の動作を制御する経路追跡工程とを有することを特徴としている。
【００１２】
また、本発明に係る障害物回避プログラムは、移動体の移動可能な領域が判別された地図に基づいて障害物を回避する障害物回避プログラムにおいて、上記地図における上記移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成工程と、上記移動体の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索工程と、上記経路に基づいて上記移動体の動作を制御する経路追跡工程とを備えることを特徴としている。
【００１３】
また、本発明に係る移動型ロボット装置は、移動可能な領域が判別された地図に基づいて障害物を回避する移動型ロボット装置において、上記地図における自身の方向毎に層を設け、各層で共通の地図を有する状態空間を構成する状態空間構成手段と、自身の動作に関する評価値に基づいて上記状態空間内に経路を生成する経路探索手段と、上記経路に基づいて自身の動作を制御する経路追跡手段とを備えることを特徴としている。
【発明の効果】
【００１４】
移動体の方向毎に層を設け、各層で共通の地図を有する状態空間を構成し、動作に関する評価値に基づいて上記状態空間内に経路を生成することにより、干渉チェック等における計算量を削減することができる。また、効率的に経路を計画するとともに最適な行動を発現することができる。
【発明を実施するための最良の形態】
【００１５】
以下、図面を参照しながら本発明の実施の形態について説明する。この実施の形態は、本発明に係る障害物回避装置を２足歩行のロボット装置に適用したものである。
【００１６】
図１は、ロボット装置１の概略を示すブロック図である。ロボット装置１の頭部ユニット２０３には、２台のＣＣＤカメラ１０Ｒ，１０Ｌが設けられ、このＣＣＤカメラ１０Ｒ，１０Ｌの後段には、ステレオ画像処理装置１２が設けられている（ただし、Ｒ及びＬは、右及び左の各々を示す接尾辞である。以下において同じ）。２台のＣＣＤカメラにより撮像された右目画像１１Ｒ、左目画像１１Ｌは、ステレオ画像処理装置１２に入力される。ステレオ画像処理装置１２は、各画像１１Ｒ，１１Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像（ＹＵＶ：輝度Ｙ、ＵＶ色差）１３及び視差画像（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）１４をフレーム毎に左右交互に算出する。ここで、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
【００１７】
このカラー画像１３及び視差画像１４はロボット装置１の体幹部２０２に内蔵されたＣＰＵ（制御部）１５に入力される。また、ロボット装置１の各関節にはアクチュエータ１６が設けられており、ＣＰＵ１５からの指令となる制御信号１８が供給されて、その指令値に応じてモータを駆動する。各関節（アクチュエータ）には、ポテンショメータが取り付けられ、モータ駆動時の回転角がＣＰＵ１５に送られる。このアクチュエータ１６に取り付けられたポテンショメータ、足底に取り付けられたタッチセンサ及び体幹部２０２に取り付けられたジャイロ・センサ等の各センサ１７は、現在の関節角度、設置情報、及び姿勢情報等のロボット装置１の状態を計測し、センサデータ１９としてＣＰＵ１５へ出力する。ＣＰＵ１５は、ステレオ画像処理装置１２からのカラー画像１３及び視差画像１４と、アクチュエータ１６全ての関節角度等のセンサデータ１９とが入力されて、制御信号１８によりロボット装置１の動作を制御する。
【００１８】
本実施の形態のロボット装置１におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置１が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。
【００１９】
図２は、ロボット装置１に搭載される障害物回避装置２の機能ブロック図である。なお、これらの機能は、障害物回避プログラムを基にＣＰＵ１５が実行するものである。
【００２０】
障害物回避装置２は、ＣＣＤカメラ１０Ｒ，１０Ｌにより撮像された画像情報に基づいて環境内の障害物情報を示す環境地図を作成する環境地図作成部３と、環境地図を格納する環境地図データベース３ａと、環境地図を用いて障害物と衝突しない経路を生成する経路計画部４と、生成された経路上に沿ってロボット装置１の行動を制御する経路追跡部５とを備えている。
【００２１】
環境地図作成部３は、図３に示すように３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）３１と、ステレオビジョンシステム３１からのステレオデータＤ１が入力され、このステレオデータＤ１から環境内の平面を検出する平面検出器（Plane Segmentation）３２と、平面検出器３２から出力される平面データＤ２から後述する環境地図を更新し、環境地図データＤ５を出力する環境地図生成部（Update GridMap & HeightMap）３３とを備える。
【００２２】
この環境地図作成部３は、まずステレオビジョンシステム３１によって外界を観測し、ステレオ視カメラの視差画像によって算出される３次元距離情報であるステレオデータＤ１を画像として出力する。すなわち、人間の両眼に相当する左右２つのカメラからの画像入力を画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。そして、ステレオデータＤ１から平面検出器３２によって平面を検出することによって環境内に存在する複数の平面を認識する。また、環境地図生成部３３によって平面データＤ２からステレオビジョンシステム３１で取得された周辺の環境を水平方向及び垂直方向に所定の大きさの格子（Grid）に分割された環境地図を作成する。
【００２３】
この環境地図は、環境内の障害物の情報を有しており、例えば所定の閾値よりも高い位置にあるものを障害物として床面からの距離を算出し、この障害物データに基づいた障害物の占有確率に応じて各グリッドが着色されたものである。例えば、各グリッドは白と黒のモノトーンで着色され、黒味が強くなるにつれて障害物が存在する確率が高くなるようにする。
【００２４】
次に、環境地図に基づいて経路計画を行う経路計画部４について説明する。経路計画部４は、後述する状態空間内のスタートの状態を表す点から始めて、行動による遷移の連続によってゴールの状態を表す点まで到達する軌跡と一連の行動とを計画するものである。
【００２５】
経路計画部４は、図４に示すようにロボット装置１の歩行動作に関する情報を有する行動空間データベース４１と、環境地図とロボット装置１の姿勢を行動空間データベースに応じて離散化し、状態空間を構成する状態空間構成部４２と、状態空間を用いて現在地と目的地を結ぶ安全な経路を生成する経路探索部４３とを備えている。
【００２６】
行動空間データベース４１には、直進、回転、横歩きなど、歩行の基本的な要素（歩行プリミティブ＝行動）について、各行動によって遷移する移動量や各行動の遷移に対するコストなどが格納されている。このコストは、後述するが、行動の実行に必要な時間、行動の安全性、行動の変更に対するペナルティ、行動に関する制限などに基づいて定義されている。
【００２７】
状態空間構成部４２は、環境地図を用いて状態空間を構成する。状態空間とは、経路計画する対象の状態を表現する空間であり、通常、その次元数は、対象の幾何的な自由度と一致する。つまり、ロボット装置１の姿勢状態数と一致する。また、状態空間の中では、ロボット装置１の幾何的な状態は、ロボット装置１を円柱モデルとし、環境側を半径分拡大することにより１点で表現することができる。
【００２８】
ここで問題となるのが、この状態空間をどのように離散化するかということである。細かい離散化を行うことによって、経路の精度は向上するが、計算量は飛躍的に増加する。
【００２９】
そこで、本実施の形態では、状態空間を実空間のグリッド表現とロボット装置の離散的な姿勢表現によって構成する。例えば、図５に示すように姿勢表現として回転方向に４５度毎、８方向に離散化し、これら８方向に対してそれぞれレイヤを設けるとともに、各レイヤを同じ環境地図にて構成する。すなわち、（ａ）、（ｂ）…のように地図におけるロボット装置１の４５度毎の方向に対し、それぞれレイヤを有して構成されている。
【００３０】
このように各レイヤで同じ環境地図を用いて状態空間を構成することにより、行動によってロボット装置１の状態が遷移する際、すなわちレイヤ間を遷移する際、特別な干渉チェックを必要としないため、計算量を著しく減少させることができる。
【００３１】
続いて、ロボット装置１の状態を遷移させる行動について説明する。行動の数は、ロボット装置１の行動の自由度と一致するが、本実施の形態では、ロボット装置１が図６に示す（ａ）Forward、（ｂ）L-Turn、（ｃ）R-Turn、（ｄ）L-Forward、（ｅ）R-Forward、（ｆ）L-Side、（ｇ）R-Side及び（ｈ）Backwardの８つの行動を発現するものとする。これらの行動は、図７に示すようにロボット装置１を４５度回転させた状態に分けることができる。このため、ロボット装置１を上述した状態空間内に表し、ロボット装置１が行動を実施することによって状態空間内の計画対象（ロボット装置１）が遷移する。この場合、ロボット装置１の円柱モデルは、行動に要する最小半径とすることが好ましい。つまり、ここでは、円柱モデルの直径をロボット装置１が横歩き可能な幅とすることが好ましい。
【００３２】
次に、経路探索部４３について説明する。経路探索部４３は、状態空間構成部４２により構成された状態空間を用いて経路を探索する。経路探索は、例えば、経路全体のコストを最小化するＡ＊探索と呼ばれる方法を用いることができる。具体的には、スタートノードから始めて、式（１）に示す評価関数ｆの評価値（コスト）が最小となるノードを順次展開することにより経路を探索する。ここで、ノードは状態空間内の各グリッドである。
【００３３】
【数１】
2006205348.tif 000003
ここで、g(n)は現在のノードでのコスト、h(n,goal)は現在のノードからゴールまでの経路の見積もりコスト、及びαは重み係数である。
【００３４】
このコストは、行動空間データベース４１に格納され、行動の安全性（Obstacle Cost）、行動の実行に必要な時間（Action Cost）、行動の変更に対するペナルティ(Action Change Cost)、行動に関する制限（Environment Cost）により構成され、実験等による経験則によって適切に設定される。つまり、経路探索は、Obstacle Cost、Action Cost、Action Change Cost及びEnvironment Costの総和が最小となるように行われるため、式（２）のように表すことができる。
【００３５】
【数２】
2006205348.tif 000004
Obstacle Costは、行動を実行する行動空間に基づいて定義される。このObstacle Costは、例えば、図８に示すように障害物との距離に基づいて定義することができる。ここで、行動が安全か否かの境界線、すなわち、行動により障害物と衝突しない安全距離がTurn＞Walk＞Side Walkの順番で設定されている。つまり、行動に対するObstacle Costの閾値は、Side Walk＞Walk＞Turnの順番で設定されている。このような定義により、例えばSide Walkは、障害物との距離が安全距離以上の場合に発現可能である。
【００３６】
Action Costは、行動に要する時間に基づいて定義される。行動に要する時間は、例えば、Side Walk＞Turn＞Walkの順番に設定される。
【００３７】
また、Action Change Costは、行動を変更する際にかかる時間、つまり行動の遷移に要する時間に基づいて定義され、行動に制約を設けることができる。このAction Change Costは、例えば一定値で定義される。
【００３８】
Environment Costは、環境属性に基づいて定義され、経路に対して不連続な制約条件を付加することを可能とする。例えば、環境属性として、平面、階段、障害物、観測領域、未観測領域などを利用し、階段で可能な行動は前進のみとし、階段における旋回や後進には大きなコストを設定する。また、未観測の属性を持つグリッドでの横歩き及び後進にも大きなコストを設定する。例えば、図９に示すように階段（Stairs）領域では（ａ）Forwardの１つの行動のみ、未観測領域では、（ａ）Forward、（ｂ）L-Turn、（ｃ）R-Turn、（ｄ）L-Forward、（ｅ）R-Forwardの５つの行動のみに制限する。これにより、未観測領域に対して観測に適さない形で進入する経路を生成することを防ぐことができる。
【００３９】
また、各行動は、後述するコストを定義することにより領域毎に制限することができる。例えば、図９に示すように階段（Stairs）領域では（ａ）Forwardの１つの行動のみ、未観測領域では、（ａ）Forward、（ｂ）L-Turn、（ｃ）R-Turn、（ｄ）L-Forward、（ｅ）R-Forwardの５つの行動のみに制限する。このような制限は、コストを高く設定することによって行われる。
【００４０】
このようにコストをObstacle Cost、Action Cost、Action Change Cost及びEnvironment Costにより構成し、このコストに基づいて経路計画を行うことにより、２足歩行であるロボット装置１のノンホロノミック性を保持するとともに高速な経路計画を実現することができる。
【００４１】
ここで、図１０に示す簡単な例を用いて経路探索について説明する。なお、ここでは、評価関数fにおけるg(ｎ)は実際の移動距離と４近傍の障害物数との和とし、h(n,goal)は現在のノードからゴールまでの推定距離とする。
【００４２】
先ず、スタート位置であるノード０のコスト、ｆ(0)を求める。図１１（ａ）に示すように、ノード０において実際の移動距離は０であり、また、４近傍の障害物数も０である（地図の境界は含まない）。また、ゴールまでの推定距離は４であるため、ｆ(0)＝４となる。次に、ノード０から左方向に１移動したノード１、前方向に１移動したノード２及び右方向に１移動したノード３について、図１１（ｂ）のようにそれぞれのコストを求める。ノード０の展開において、ノード２のコスト（ｆ(2)）が最小であるから、ノード２を前後左右の４方向に展開する（コストを計算する）。ノード２の展開では、図１１（ｃ）に示すように、ノード０のコスト（f(0)）は算出され登録済みであり、ノード６は衝突してしまうので計算する必要はない。ノード２の展開では、ノード４が最小のコストなので、次はノード４を展開する。このように、コストが最小であるノードをゴールまで順次展開していくことにより経路を生成することができる。
【００４３】
続いて、本実施の形態における経路探索を、図１２に示すフローチャートを用いて説明する。先ず、ステップＳ１においてノードＮを現在地とする。各ノードにはノード情報が存在する。ノード情報は、ノード番号、ノード位置情報、既探索フラグ（Visited Flag）、最小経路コスト（Cost）、親ノード（Parent）、到達アクション（Action）を含んでいる。
【００４４】
ステップＳ２では、ノードＮの既探索フラグ（Visited Flag）をTrueとし、ノードＮを展開する。次に、ノードＮにおいて行動Ａ＝Ａ１を発現した場合（ステップＳ３）に移動する遷移先Ｎｘを決定し（ステップＳ４）、遷移先Ｎｘに対し経路コストCostを計算する（ステップＳ５）。遷移先Ｎｘの経路コスト（Cost）は、ノードＮの最小経路コストとノードＮからの遷移コストとの和によって計算される。
【００４５】
ステップＳ６では、遷移先Ｎｘの親ノード（Nx.Parent）の登録の有無を判別する。遷移先Ｎｘに親ノードがある場合、ステップＳ８に進む。また、遷移先Ｎｘに親ノードNx.Parentがない場合、ステップＳ７に進み、ステップＳ５にて計算された遷移先Ｎｘの経路コスト（Cost）と遷移先Ｎｘの最小経路コスト（Nx.Cost）が比較される。そして、ステップＳ５にて計算された遷移先Ｎｘの経路コスト（Cost）が登録されている遷移先Ｎｘの最小経路コスト（Nx.Cost）以下の場合、ステップＳ８に進む。また、大きい場合は、遷移先として適切でないためステップＳ１２へ進む。
【００４６】
ステップＳ８では、登録された遷移先Ｎｘの最小経路コスト（Nx.Cost）に計算された遷移先Ｎｘの経路コスト（Cost）を代入して更新する。そして、遷移先Ｎｘの親ノード（Nx.Parent）にＮを登録し（ステップＳ９）、遷移先Ｎｘに到達した到達行動に行動Ａを登録し（ステップＳ１０）、遷移先の候補リストにＮｘを登録し（ステップＳ１１）、ステップＳ１２へ進む。
【００４７】
ステップＳ１２では、行動Ａ＝Ａ２，Ａ３,,,Ａｎを順に代入し、ステップＳ２乃至ステップＳ１１の処理を行う。ステップＳ１３では、ステップＳ１２において全ての行動について終了したか否かが判別され、終了したと判別された場合ステップＳ１４へ進む。また、終了していない場合ステップＳ４に戻る。
【００４８】
ステップＳ１４では、候補リストに登録されたノードの有無を判断する。候補リスト中に何も登録されていない場合、ノードＮにおける展開は失敗である。
【００４９】
また、候補リストに登録されたノードがある場合、候補リストから最小のコストを持つノードを新しいＮとして選択する（ステップＳ１５）。そして、新しいＮが目的地か否かを判別する（ステップＳ１６）。
【００５０】
ステップＳ１６にて新しいＮが目的地の場合、目的地から親ノードを辿ることによって、経路を生成する（ステップＳ１７）。このように、経路を探索することにより、最適な経路で行動することができる。
【００５１】
ここで、コストを用いた干渉チェックについて説明する。例えば、図１３に示すような障害物が存在する場合、環境地図作成部３は、入力されたカラー画像１３及び視差画像１４より上述の処理を行い、図１４に示すような環境地図を生成する。この図１４では、ロボット装置１と同じ高さの床面は白色で示され、高さが異なる面には高さに応じて着色されている。また、障害物の可能性の高いグリッドには黒色、階段及び高さの低い障害物には×印を用いて表している。また、図中Ａの領域は、未観測領域である。
【００５２】
図１５は、図１４に示す環境地図を、コストを用いて表現したものである。図１５はコストを白黒の濃淡で表し、色が濃いほどコストが高いことを示している。ここで、障害物に近いほどコストが高い。つまり、行動が制限されている。また、白色の領域は、ロボット装置１の行動が制限されない自由空間領域を示している。
【００５３】
このように、経路計画部４は、ロボット装置１の各状態に同じ環境地図を用いた状態空間を構成することにより、ロボット装置１の行動における干渉チェックや経路計画における計算量を減少させることができる。
【００５４】
また、経路追跡部５は、経路計画部４により生成された経路情報にロボット装置１が行うことができる行動の情報が含まれているため、容易にロボット装置１の行動を制御し、適切に障害物を回避することができる。
【００５５】
以上のように、障害物回避装置２は、状態空間の設計、行動空間の設計及び探索におけるコストを適切に定義することにより、ロボット装置１を円柱モデルとして扱っても効率よく経路を計画し、障害物を回避することができる。
【００５６】
次に、障害物回避装置２が搭載されるロボット装置１について図１６乃至図１８を参照して、さらに詳細に説明する。図１６は、図１に示すロボット装置１の詳細な外観を示す斜視図である。図１６に示すように、ロボット装置１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている。
【００５７】
このロボット装置１が具備する関節自由度構成を図１７に模式的に示す。頭部ユニット２０３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
【００５８】
また、上肢を構成する各々の腕部ユニット２０４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール軸１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
【００５９】
また、体幹部ユニット２０２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
【００６０】
また、下肢を構成する各々の脚部ユニット２０５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足底１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置１の股関節位置を定義する。人体の足底１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
【００６１】
以上を総括すれば、ロボット装置１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
【００６２】
上述したようなロボット装置１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うこと等の要請から、アクチュエータは小型かつ軽量であることが好ましい。
【００６３】
このようなロボット装置１は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット２０２等に備える。図１８は、ロボット装置１の制御システム構成を示す模式図である。図１８に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール２００と、アクチュエータ３５０の駆動等、ロボット装置１の全身協調運動を制御する運動制御モジュール３００とで構成される。
【００６４】
思考制御モジュール２００は、情緒判断や感情表現に関する演算処理を実行するＣＰＵ（Central Processing Unit）２１１や、ＲＡＭ（Random Access Memory）２１２、ＲＯＭ（Read Only Memory）２１３及び外部記憶装置（ハード・ディスク・ドライブ等）２１４等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。
【００６５】
この思考制御モジュール２００は、画像入力装置２５１から入力される画像データや音声入力装置２５２から入力される音声データ等、外界からの刺激等にしたがって、ロボット装置１の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット装置１の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置２５１は、例えばＣＣＤ（Charge Coupled Device）カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。また、音声入力装置２５２は、例えばマイクロホンを複数備えている。
【００６６】
思考制御モジュール２００は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール３００に対して指令を発行する。
【００６７】
一方の運動制御モジュール３００は、ロボット装置１の全身協調運動を制御するＣＰＵ３１１や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。
【００６８】
この運動制御モジュール３００には、図１８に示したロボット装置１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インターフェース（Ｉ／Ｆ）３１０経由で接続されている。ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組合せによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。
【００６９】
思考制御モジュール２００と運動制御モジュール３００は、共通のプラットフォーム上で構築され、両者間はバス・インターフェース２１０，３１０を介して相互接続されている。
【００７０】
運動制御モジュール３００では、思考制御モジュール２００から指示された行動を体現すべく、各アクチュエータ３５０による全身協調運動を制御する。すなわち、ＣＰＵ３１１は、思考制御モジュール２００から指示された行動に応じた動作パターンを外部記憶装置３１４から取り出し、又は、内部的に動作パターンを生成する。そして、ＣＰＵ３１１は、指定された動作パターンにしたがって、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各アクチュエータ３５０に転送する。
【００７１】
また、ＣＰＵ３１１は、姿勢センサ３５１の出力信号によりロボット装置１の体幹部ユニット２０２の姿勢や傾きを検出するとともに、各接地確認センサ３５２，３５３の出力信号により各脚部ユニット２０５Ｒ／Ｌが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット装置１の全身協調運動を適応的に制御することができる。更に、ＣＰＵ３１１は、ＺＭＰ位置が常にＺＭＰ安定領域の中心に向かうように、ロボット装置１の姿勢や動作を制御する。
【００７２】
また、運動制御モジュール３００は、思考制御モジュール２００において決定された意思通りの行動がどの程度発現されたか、すなわち処理の状況を、思考制御モジュール２００に返すようになっている。このようにしてロボット装置１は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
【００７３】
続いて、上述のロボット装置１に障害物回避装置を搭載させた結果を、図１９乃至図２８を用いて説明する。環境として図１９に示すように障害物が配置された４ｍ×１ｍのコースを用意した。ここで、障害物Ａは図２０に示す角材であり、長さ４０ｃｍ、幅９ｃｍ、高さ４ｃｍとなっている。また、図２１に示す障害物Ｂは踏面が幅３３ｃｍ、奥行き１２ｃｍ、蹴り上げ３ｃｍの階段を有する台である。ここで、スタート地点を図２２に示すように障害物ｆと障害物ｇの前とし、ゴールを障害物Ｂの台上までとする。
【００７４】
図２２に示すような位置のロボット装置１は、頭部ユニット２０３に搭載された２台のＣＣＤカメラ１０Ｒ，１０Ｌにより、ステレオ画像を取得し、上述の処理を行うことによって、図２３に示すような経路を生成する。この図２３では、コストを白黒の濃淡で表し、色が濃いほどコストが高いことを示している。ここで、障害物ｆと障害物ｇの間は、ロボット装置１の行動が制限されない自由空間領域を示す白色ではないため、ロボット装置１は行動が制約され、横歩きする。このように、環境チェックを横歩きに必要な幅だけ拡大させた環境地図を用い、コストの横歩きの閾値を前進歩行よりも高く定義することによって、従来では横歩きで移動することが困難であった場所でも、高速に経路計画し、行動を制御して移動することができる。
【００７５】
また、障害物ｆと障害物ｇの間を横歩きし、図２４に示すように障害物Ａの前では、ロボット装置１は、例えば距離データの分布を解析することにより、踏面や床面との段差等を認識し、図２５に示す経路計画を行う。この段差の領域Ａでは、上述したように例えばEnvironment Costによって行動が制約されている。また、障害物Ａの段差を乗り越えたロボット装置１は、図２６に示すように経路計画し、図２７の障害物Ｂの前まで行動する。
【００７６】
障害物Ｂの前では、ロボット装置１は、例えば距離データの分布を解析することにより、階段を検出し、踏面や段差を認識し、ゴールである障害物Ｂの台の上までの経路を図２８に示すように計画する。
【００７７】
このように、障害物回避装置２を搭載したロボット装置１は、自律的にゴールまでの経路を計画及び行動し、障害物を回避することができる。
【図面の簡単な説明】
【００７８】
【図１】本発明の具体例として示すロボット装置の概略を説明するブロック図である。
【図２】本実施の形態における障害物回避装置を説明する機能ブロック図である。
【図３】環境地図作成部を説明する機能ブロック図である。
【図４】経路計画部を説明する機能ブロック図である。
【図５】状態空間を説明するための模式図である。
【図６】上記ロボット装置の状態を説明するための模式図である。
【図７】行動空間を説明するための模式図である。
【図８】行動におけるコストを説明するための図である。
【図９】コストを説明するための図である。
【図１０】経路探索例を説明するための図である。
【図１１】図９に示す経路探索例における計算方法の一例を説明するための図である。
【図１２】本実施の形態における経路探索方法を説明するためのフローチャートである。
【図１３】上記ロボット装置によって認識される実空間を説明する模式図である。
【図１４】図１２に示す実空間と同じ領域を表す環境地図を説明する模式図である。
【図１５】図１２に示す実空間と同じ領域の干渉チェックについて説明する模式図である。
【図１６】本発明の実施の形態におけるロボット装置の外観を示す斜視図である。
【図１７】上記ロボット装置が具備する関節自由度構成を模式的に示す図である。
【図１８】上記ロボット装置の制御システム構成を示す模式図である。
【図１９】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２０】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２１】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２２】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２３】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２４】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２５】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２６】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２７】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２８】上記ロボット装置に障害物回避装置を搭載させた結果を説明するための模式図である。
【図２９】従来の経路探索を説明するための模式図である。
【図３０】従来の経路探索を説明するための模式図である。
【図３１】従来の経路探索を説明するための模式図である。
【符号の説明】
【００７９】
１    ロボット装置、  ２    障害物回避装置、  ３    環境地図作成部、  ４    経路計画部、  ５    経路追跡部、  ４１    行動空間データベース、  ４２    状態空間構成部、  ４３    経路探索部
【図１】
2006205348.tif 000005
【図２】
2006205348.tif 000006
【図３】
2006205348.tif 000007
【図４】
2006205348.tif 000008
【図５】
2006205348.tif 000009
【図６】
2006205348.tif 000010
【図７】
2006205348.tif 000011
【図８】
2006205348.tif 000012
【図９】
2006205348.tif 000013
【図１０】
2006205348.tif 000014
【図１１】
2006205348.tif 000015
【図１２】
2006205348.tif 000016
【図１３】
2006205348.tif 000017
【図１４】
2006205348.tif 000018
【図１５】
2006205348.tif 000019
【図１６】
2006205348.tif 000020
【図１７】
2006205348.tif 000021
【図１８】
2006205348.tif 000022
【図１９】
2006205348.tif 000023
【図２０】
2006205348.tif 000024
【図２１】
2006205348.tif 000025
【図２２】
2006205348.tif 000026
【図２３】
2006205348.tif 000027
【図２４】
2006205348.tif 000028
【図２５】
2006205348.tif 000029
【図２６】
2006205348.tif 000030
【図２７】
2006205348.tif 000031
【図２８】
2006205348.tif 000032
【図２９】
2006205348.tif 000033
【図３０】
2006205348.tif 000034
【図３１】
2006205348.tif 000035