JP2007041657A - 移動体制御方法および移動体 - Google Patents

Abstract

【課題】 移動体が利用可能な移動動作が変化した場合でも、適切な移動動作を選択してパス上を移動できる移動体制御方法を提供する。
【解決手段】 パス決定部３６が、ＣＰＵ１５が機能として利用可能な処理部３５＿１〜３５＿６等を特定し、その利用な処理部を基に、目標位置までの複数のパスを動的に特定する。そして、パス決定部３６が、処理部３５＿１〜３５＿６を順次選択的に指示してロボット１に走行動作を行わせる。
【選択図】 図５

Description

本発明は、撮像画像を基に移動する移動体を制御する移動体制御方法と、その移動体に関する。

例えば、自律型ロボットは、産業用ロボットと異なり、周囲の外部状態やロボット自身の内部状態に応じて自律的に動作を行うことのできるロボットである。例えば、自律型ロボットは、歩行動作では、外部の障害物を検出して障害物を回避するような行動経路を計画することができなくてはならない。従来のロボットのなかには、自身のおかれた環境の障害物地図を作成し、これに基づいて行動経路を決定できるロボットも提案されている。

米国特許第５００６９８８号公報 特開２００３−２６９９３７号公報

ところで、従来のロボットは、自らが備える機能、例えば、通常歩行、階段走行、よじ登り動作、ジャンプ動作などを行う機能が正常であるとの前提でロボットが移動するパスを決定し、ロボットを動作させている。
ところで、上述した機能は、その一部が故障する場合があるが、従来のロボットでは、全ての機能を正常に動作するという前提で、パス上の移動過程で利用する機能を順次決定するため、故障した機能が選択され、ロボットがパスを移動できない場合がある。

本発明は上述した従来技術の問題点を解決するために、移動体が利用可能な移動動作が変化した場合でも、適切な移動動作を選択してパス上を移動できる移動体制御方法および移動体を提供することを目的とする。

上述した従来技術の問題点を解決し、上述した目的を達成するため、第１の観点の発明の移動体制御方法は、移動体が目標位置まで移動する動作を制御する移動体制御方法であって、前記移動体の現在位置から前記目標位置までのパスの状態を検出する第１の工程と、前記移動体が実行可能な複数の移動動作を特定する第２の工程と、前記第１の工程で検出したパスの状態に基づいて、前記第２の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように制御する第３の工程とを有する。

第２の観点の発明の移動体は、異なる複数の撮像位置からの撮像対象領域の撮像結果を生成する撮像手段と、走行手段と、前記撮像手段が生成した前記撮像結果を基に、前記移動体の現在位置から前記目標位置までのパスの状態を検出し、前記移動体が実行可能な複数の移動動作を特定し、前記検出したパスの状態に基づいて、前記第２の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように前記走行手段を制御する制御手段とを有する。

本発明によれば、移動体が利用可能な移動動作が変化した場合でも、適切な移動動作を選択してパス上を移動できる移動体制御方法および移動体を提供することができる。

以下、本発明の実施形態に係わるロボットについて説明する。
先ず、本実施形態の構成要素と、本発明の構成要素との対応関係を説明する。
図５に示す環境クラス分類部３４の処理が本発明の第１の工程の一例であり、図２２に示すパス決定部３６のステップＳＴ６１の処理が本発明の第２の工程の一例であり、ステップＳＴ６２〜ＳＴ６４、図２４および図２５の処理が本発明の第３の工程の一例である。
また、図２３の処理が、本発明の第４の工程の一例である。

また、図１に示すＣＣＤカメラ１０Ｒ，１０Ｌが本発明の撮像手段の一例であり、図３および図４に示す脚部などが本発明の走行手段の一例であり、図１に示すＣＰＵ１５が本発明の制御手段の一例である。

本実施形態のロボット１は、ロボット１では、図５等に示すように、ＣＰＵ１５が機能として利用可能な処理部３５＿１〜３５＿６等を特定し（図２２に示すステップＳＴ６１）、その利用な処理部を基に、目標位置までの複数のパスを動的に特定する（ステップＳＴ６２）。
これにより、例えば、処理部３５＿１〜３５＿６あるいはそれによって制御されるアクチュエータ群１６などの機械部位が故障した場合に、その故障した機能に対応した処理部による制御を利用しないで目標位置に達するパスを選択することができる。

図１は、本発明の実施形態のロボット１の機能ブロック図である。
図１に示すように、ロボット１は、例えば、ＣＣＤカメラ１０Ｒ，１０Ｌ、ステレオ画像処理装置１２、ＣＰＵ１５、アクチュエータ群１６およびセンサ群１７を有する。
ＣＣＤカメラ１０Ｒ，１０Ｌは、それぞれ撮像結果に応じた右画像データＳ１０Ｒおよび左画像データＳ１０Ｌをステレオ画像処理装置１２に出力する。

ステレオ画像処理装置１２は、ＣＣＤカメラ１０Ｒ，１０Ｌから入力した右画像データＳ１０Ｒと左画像データＳ１０Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像データ（ＹＵＶ：輝度Ｙ、ＵＶ色差）Ｓ１２ａ及び視差画像データ（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）Ｓ１２ｂをフレーム毎に左右交互に算出する。
ここで、視差とは、空間中のある点が左側カメラ及び右側カメラに写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
カラー画像データＳ１２ａ及び視差画像データＳ１２ｂは、ＣＰＵ（制御部）１５に入力される。
ＣＰＵ１５は、ステレオ画像処理装置１２から入力したカラー画像データＳ１２ａ及び視差画像データＳ１２ｂを用いて、後述するソフトウェア構成によって、画像処理を行う。

アクチュエータ群１６は、ロボット１の動きを駆動する各種のアクチュエータである。
アクチュエータ群１６は、ＣＰＵ１５からの制御信号Ｓ１５によって駆動される。
センサ群１７は、ロボット１の所定箇所に設置された赤外線センサなどの非接触型センサや接触型センサなどである。
センサ群１７は、検出信号Ｓ１７をＣＰＵ１５に出力する。

以下、図１に示すロボット１の構成をさらに詳細に説明する。
図２に示すように、ロボット１は、２足歩行タイプであり、日常生活上の様々な場面における人的活動を支援する実用ロボットである。
ロボット１は、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボットである。ロボットは、３次元環境クラスデータを生成し、これを基に行動経路を決定する。

図２に示すように、ロボット１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている。

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

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

また、下肢を構成する各々の脚部ユニット２０５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足底１２１とで構成される。
本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット１の股関節位置を定義する。人体の足底１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
以上を総括すれば、ロボット１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。

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

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

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

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

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

運動制御モジュール３００は、ロボット１の全身協調運動を制御するＣＰＵ３１１（図１に示すＣＰＵ１５）や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。
この運動制御モジュール３００には、図２に示したロボット１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インターフェース（Ｉ／Ｆ）３１０経由で接続されている。
ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組合せによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。

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

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

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

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

以下、図１および図４に示すＣＰＵ１５（ＣＰＵ３１１）の機能を説明する。
図５は、図１および図４に示すＣＰＵ１５の機能ブロック図である。
図５に示すように、ＣＰＵ１５は、例えば、距離データ生成部３１、平面検出部３２、グリッドマップ生成部３３、環境クラス分類部３４、通常走行処理部３５＿１、階段走行処理部３５＿２、注意走行処理部３５＿３、這動作処理部３５＿４、ステップオーバ処理部３５＿５、よじ登り／ジャンプ処理部３５＿６，パス決定部３６、並びに動き制御部３７を有する。

［距離データ生成部３１］
距離データ生成部３１は、図１に示すステレオ画像処理装置１２から入力したカラー画像データＳ１２ａ及び視差画像データＳ１２ｂを基に、ロボット１から画像（環境）内の対象物までの３次元距離を示す距離データ（ステレオデータ）Ｓ３１を生成し、これを平面検出部３２およびグリッドマップ生成部３３に出力する。
距離データ生成部３１は、例えば、図６に示すような画像（環境）内の距離データＳ３１を生成する。図６に示す実空間Ｓ５００には、ロボット１が登行可能な平面ＰＬ５０１とＰＬ５０２とを備える階段（Staircase）ＳＴ５０３と、ロボット１が登行不可能な障害物（Obstacle）ＯＢ５０４，５０５とが示されている。
また、図７は、ロボット１が外界を撮影している様子を示す模式図である。
ここでは、床面（基準面）をｘ−ｙ平面とし、高さ方向をｚ軸としたとき、図７に示すように、画像入力部（ステレオカメラ）を頭部ユニット２０３に有するロボット１の視野範囲は、ロボット１の前方の所定範囲となる。ロボット１は、視野範囲にある環境を認識して距離データＳ３１を生成する。

［平面検出部３２］
平面検出部３２は、距離データ生成部３１から入力した距離データＳ３１から環境内に存在する複数の平面を検出し、それを示す平面データＳ３２をグリッドマップ生成部３３に出力する。
ここでの平面の検出方法としては、線分拡張法、ハフ変換等を利用した公知の平面検出技術を適用することができる。ノイズを含む距離データから階段のように複数平面を検出する代表例として、線分拡張法による平面検出を行うと正確に平面を検出することができる。

線分拡張法は、視野内において支配的な平面だけでなく、例えば階段など複数の平面が
存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出
する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさ
せることにより観測ノイズに対してロバストな平面検出結果を得ることができる。

平面検出部３２は、図８に示すように、画像を構成する距離データから同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部５１と、画像内に含まれる、線分抽出部５１よって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部５２とを有する。
領域拡張部５２は、線分群から同一平面上に存在すると推定される任意の３本の線分を選択し、これらから基準平面を求める。そして、選択した３本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新するとともに基準平面の領域を拡張する。

線分抽出部５１は、その距離画像における列又は行毎の各データ列において、３次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて１以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群Ｓ５１を領域拡張部５２に出力する。

領域拡張部５２は、この線分Ｓ５１において、同一の平面に属すると推定される線分を３本選択し、これらから基準平面としての種となる平面を求める。この種となる平面の領域（領域種：seed region）に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群の平面データＳ３２を出力する。

平面検出部３２は、ハフ変換のように、取得した画像に含まれる支配的な平面を検出するのみならず、階段など複数平面が含まれている場合であっても平面を検出可能とするため、線分拡張法により平面を検出する。そして、この際、距離データ点の分布に応じて線分を生成することにより、計測ノイズに対してロバストな検出結果を得ることができるものである。

図９は、線分拡張法による平面検出方法を説明する図である。
線分拡張法による平面検出では、図９に示すように、まず、焦点Ｆから撮影された画像２０において、行方向又は列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row：イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分からなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。

図１０は、線分拡張法による平面検出処理を示すフローチャートである。
図１０に示すように、まず、平面検出部３２は、距離データＳ３１を入力し（ステップＳＴ１）、距離画像の行方向（又は列方向）の各画素列において同一平面に属すると推定されるデータ点から線分を求める（ステップＳＴ２）。
そして、平面検出部３２は、これらの線分群のなかから同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める（ステップＳＴ３）。このステップＳ３では、まず、平面検出部３２は、平面の種となる領域（以下、領域種（seed region）という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の１ラインを含む３本の線分が同一平面にあることを条件とする。ここで、平面検出部３２は、選択した３本の線分からなる領域種が属する平面を基準平面とし、３本の線分から平均して求まる平面を求めておく。また、３本の線分からなる領域を基準平面領域とする。

そして、平面検出部３２は、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し（領域拡張処理）、追加した線分を含めたものとして上記基準平面を更新し（平面更新処理）、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域（３本の線分）が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群のなかから同一平面を構成するものを連結する。そして、本実施の形態においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップＳＴ４として更に設け、最終的な平面とするが、詳細は後述する。

ここで、３次元距離データから線分を検出し、これを同一平面毎にまとめた領域を１つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本実施の形態においては、ステップＳＴ２における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しようとしても、距離データの精度に応じて閾値を変更しなければ、over-segmentation又はunder-segmentationなどもの問題が生じてしまう。そこで、本実施の形態においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。

上述した平面検出処理によって検出される平面について、図１１、図１２を用いて説明する。平面検出部３２によって検出される平面が、例えば、図１１、図１２に示す階段ＳＴＡの場合について説明する。図１１（ａ）、図１２（ａ）は階段を正面からみた図であり、図１１（ｂ）、図１２（ｂ）は階段を側面からみた図であり、図１１（ｃ）、図１２（ｃ）は階段を斜めからみた図である。

本明細書においては、人間、ロボットなどが階段を昇降するために使用する面（足又は可動脚部を載せる面）を踏面といい、一の踏面からその次の踏面までの高さ（１段の階段の高さ）を蹴り上げということとする。また、階段は、地面に近い方から１段目、２段目とカウントすることとする。

図１１に示す階段ＳＴＡ１は、段数が３段の階段であり、蹴り上げ４ｃｍ、１，２段面の踏面の大きさは幅３０ｃｍ、奥行き１０ｃｍ、最上段である３段目の踏面のみ、幅３０ｃｍ、奥行き２１ｃｍとなっている。また、図１２に示す階段ＳＴＡ２も段数が３段の階段であり、蹴り上げ３ｃｍ、１，２段面の踏面の大きさは幅３３ｃｍ、奥行き１２ｃｍ、最上段である３段目の踏面のみ、幅３３ｃｍ、奥行き３２ｃｍとなっている。

［グリッドマップ生成部３３］
グリッドマップ生成部３３は、距離データ生成部３１から入力した距離データＳ３１と平面検出部３２から入力した上記平面検出データＳ３２とに基づいて、３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを生成し、これらを環境クラス分類部３４に出力する。

先ず、グリッドマップ生成部３３による３次元グリッドマップデータＧＭの生成処理について説明する。
３次元グリッドマップデータＧＭは、周囲の環境を水平方向及び垂直方向に所定の大きさの格子（グリッド；Grid）に分割して表したものである。
グリッドマップ生成部３３は、距離データ生成部３１からの距離データＳ３１と、平面検出部３２からの平面データＳ３２とに基づいて、例えば、４メートル四方、高さ１メートルの空間を、水平解像度４センチ及び垂直解像度１センチの直方体空間を１グリッド単位（１セルと記す。）とし、その集合によって表現された３次元グリッドマップデータＧＭを生成する。
グリッドマップ生成部３３は、各セルに物体によって占有されているか否かの確率を付与し、観測データに基づいて各セルの占有確率を変更させている。また、グリッドマップ生成部３３は、占有確率に閾値を設け、占有確率が閾値より大きいか否かによって環境の状態を地図として表現している。
本具体例では、高さ方向の解像度がセル分解能に依存しないため、垂直解像度は、１セ
ンチよりも粗くできる。

グリッドマップ生成部３３は、３次元グリッドで表される実空間Ｓ５００における全ての単位グリッド（以下、セルと記す。）ｐ（ｘ，ｙ，ｚ）に対して次の処理を行う。
グリッドマップ生成部３３は、撮像画像内に像として映し出された測定点（３次元グリッド）と観測中心（ステレオカメラ中心）とを結ぶ直線上には障害物が存在しないというアルゴリズムに基づいて３次元グリッドマップデータＧＭを作成及び／又は更新している。
図１３は、３次元グリッドマップデータＧＭが観測結果に応じて更新される様子をｙ軸方向からの側面図として模式的に表したものである。ロボット１の視野領域（Field of View）にないセル（斜線のセル）は、まだ測定していないセルを表す。ロボット１によって測定されたセルは黒塗りで表され、測定点にあたるセルとの間のセルは、白塗りで表されている。ハッチング領域は、未確認領域であることを示している。

グリッドマップ生成部３３は、上述のアルゴリズムに基づいて、測定対象点であるセルｐとロボットのステレオカメラ１１Ｌ，１１Ｒとを結ぶ線分の間に存在するセルに対して、エンプティ（Empty）処理を行う。続いて、測定点ｐにあたるセルに対してオキュパイド（Occupied）処理を行う。
３次元グリッドマップデータＧＭは、セルＣに対して障害物の存在確率（障害物によって占有されている確率）ｐ（ｃ）を保持しており、このエンプティ処理及びオキュパイド処理は、これら各セルに対して統計的な処理である。
エンプティ処理は、障害物の存在確率を低下させる処理であり、オキュパイド処理は障害物の存在確率を増加させる処理である。本具体例では、エンプティ処理及びオキュパイド処理の存在確率を算出する手法の一例として、ベイズの更新則を用いる。

グリッドマップ生成部３３は、エンプティ処理において式（１）により占有確率を減少させ、オキュパイド処理において式（２）により占有確率を増加させる。
式（１）は、セルＣの占有確率を示すｐ（ｃ）が“占有”という条件下にあるセルＣの確率を表しており、式（１）又は式（２）においてセルＣが占有されているか又は占有されていないかを表す確率ｐ（occ｜・・・）、ｐ（empty｜・・・）は、予め決められた閾値ｔｈである。

［数１］
ｐ（ｃ）←ｐ（ｃ｜ｅｍｐｔｙ）＝ｐ（ｅｍｐｔｙ｜ｃ）ｐ（ｃ）／｛ｐ（ｅｍｐｔｙ｜ｃ）ｐ（ｃ）＋ｐ（ｅｍｐｔｙ｜ｃ~）ｐ（ｃ~）｝
…（１）

［数２］
ｐ（ｃ）←ｐ（ｃ｜ｏｃｃ）＝ｐ（ｏｃｃ｜ｃ）ｐ（ｃ）／｛ｐ（ｏｃｃ｜ｃ）ｐ（ｃ）＋ｐ（ｏｃｃ｜ｃ~）ｐ（ｃ~）｝
…（２）

このように障害物の存在確率が与えられた３次元グリッドマップデータＧＭが示す内容を図１４に示す。本具体例では、３次元グリッドとして表される領域は、例えば、４メートル四方、水平解像度４センチ及び垂直解像度１センチを１セルとした３次元グリッドで表現されている。

図１５では、説明の便宜上、図６に示す実空間Ｓ５００と同じ領域が３次元グリッドとして表されているが、実際のロボット１は、例えば１秒間に３０回というように所定間隔毎に周囲の状態を取得していくため、ロボット１が移動している場合には、３次元グリッドとして表現される空間Ｓ５００は、時々刻々と変化することになる。取得されたセルは、見えたところ１、見えないところ０．５とする。占有確率は、１秒間に３０回測定するうちに徐々に更新されていく。

グリッドマップ生成部３３が生成する３次元グリッドマップデータＧＭは、例えば、３次元座標（ｘ、ｙ、ｚ）の各セルについて、その占有確率ＯＧ（ｘ、ｙ、ｚ）（＝ｐ（ｃ））を示している。

以下、グリッドマップ生成部３３によるフロア高さデータＦＨの生成処理を説明する。
グリッドマップ生成部３３は、２次元高さ地図（2D Height Map）のセルに対して、３次元グリッドマップデータＧＭから得られるセルの占有確率データ（障害物存在確率データ）を用いてフロア高さデータＦＨを検証する。

本具体例では、グリッドマップ生成部３３において、フロア高さデータＦＨを、３次元グリッドマップデータＧＭを用いて生成する。
３次元グリッドマップデータＧＭは、幾何学的に測定点と観測中心（ステレオカメラ中心）とを結ぶ直線上には、障害物は存在しないというアルゴリズムに基づいて作成及び／又は更新される。
グリッドマップ生成部３３は、フロア高さデータＦＨのセルの高さ情報を３次元グリッドのセルに対応付けしたときの占有確率を３次元グリッドのセルの占有確率を参照して更新する処理を行う。

図１６は、フロア高さデータＦＨを示す。
フロア高さデータＦＨは、矩形状格子で表される２次元グリッドの各セルに対して、高さ情報ｈの情報と無効フラグ（invalid Flag）とを保持している。
図１６では、図６における平面ＰＬ５０１に対応するセルに高さｈ＝３０が、平面ＰＬ５０２に対応するセルに高さｈ＝６０が与えられている。また、このセルがロボット１には登行不可能な高さ情報をもっている場合には、無効フラグ（inv）が与えられる。

フロア高さデータＦＨも３次元グリッドマップデータＧＭと同様に、例えば、４メートル四方、水平解像度４センチを１セルとした２次元グリッドで表現される。フロア高さデータＦＨでは、垂直方向の解像度は、考慮されておらず、各セルに対して高さ情報が割り当てられている。無効フラグとは、ロボット１に対して、障害物にあたることを示しているが、無効フラグを用意する代わりに、例えば、−１００００などのように空間を表現するに非現実的な値を与えることで無効、すなわちロボット１に対する障害物であることを表してもよい。

図１７は、グリッドマップ生成部３３によるフロア高さデータＦＨの生成処理を説明するフローチャートである。
グリッドマップ生成部３３は、対基準面高さ地図としてのフロア高さデータＦＨにおける高さ情報を取得し、この高さ情報を３次元グリッドマップデータＧＭの高さ座標に対応させ、対応した３次元グリッドのセルの占有確率が３次元グリッドにおいて障害物と認識するか否かを決めている閾値より大きいか否かを比較し、閾値より小さい場合、このセルは障害物でないと判断し、閾値より大きい場合、このセルは障害物であると判断し、対応する対基準面高さ地図の高さ情報を更新している。

グリッドマップ生成部３３は、全セル（ｘ、ｙ）に対してフロア高さデータＦＨを参照してグリッドの高さを取得する（ステップＳＴ１１）。
高さ情報ｈに無効フラグが立っていた場合、処理を終了する（ステップＳＴ１２；ｙｅｓ）。

一方、高さ情報ｈが有効であった場合、すなわち高さ情報ｈがあてはめられていた場合、高さ情報ｈを３次元グリッドに対応させて３次元グリッドにおけるインデックスｋを求める（ステップＳＴ１３）。続いて、３次元グリッド内のセル（ｘ、ｙ、ｚ）の障害物の存在確率ｐ（Ｃｘｙｚ）が３次元グリッドにおいて障害物と認識するか否かを決めている閾値ｔｈより大きいか否かを比較する（ステップＳＴ１４）。

閾値より小さい場合（ステップＳＴ１４；ｎｏ）、このセルは障害物でないと判断する。
このとき、グリッドマップ生成部３３においてセル（ｘ、ｙ）を３次元グリッドマップデータＧＭを作成するアルゴリズムに基づいて更新する。閾値より大きい場合（ステップＳＴ１４；ｙｅｓ）、このセルは障害物であると判断し処理を終了する。すなわち、この場合には、フロア高さデータＦＨの高さ情報ｈを更新しない。

以上の処理は、図１３に示したように、グリッドのうち視野角領域に相当するセル領域
にのみ適用することで計算量の削減が可能となる。図１３では、この視野角領域に相当するセル領域をバウンディングボックス（Bounding Box）と記している。

グリッドマップ生成部３３は、平面検出部３２による平面データＳ３２と、３次元グリッドマップデータＧＭとからフロア高さデータＦＨを更新している。
ロボット１の視野領域にある計測空間内に含まれる全ての計測点（セル）ｐに対して、計測点ｐが属する平面が水平面であって、且つ計測点ｐに対応する３次元グリッドマップデータＧＭのセルの障害物存在確率が閾値ｔｈよりも高い場合に、計測点ｐに対応する高さ情報を更新する。

グリッドマップ生成部３３によるフロア高さデータＦＨの更新処理を図１８に示す。
グリッドマップ生成部３３は、まず、視野領域にある全計測点（セル）ｐに対して、計測点ｐが属する平面情報を取得する（ステップＳＴ２１）。
平面情報としては、平面検出部３２による平面検出結果である平面データＳ３２を用いる。具体的には、計測点ｐが検出されたどの平面に属するかを表した平面ＩＤ（ｐ_ｉｄ）を取得し、このｐ_ｉｄが有効である場合には（ステップＳＴ２２；ｙｅｓ）、このｐｉｄをキーとして検出された平面群のなかから計測点ｐが属する平面情報を検索する（ステップＳＴ２３）。

次に、グリッドマップ生成部３３は、計測点ｐが属する平面が水平面であるかどうかを検証し、水平面でない場合（ステップＳＴ２４；ｎｏ）には終了する。一方、水平面である場合（ステップＳＴ２４；ｙｅｓ）、計測点ｐに対応する３次元グリッドのセルの障害物存在確率を参照する（ステップＳＴ２５）。
具体的には、グリッドマップ生成部３３は、フロア高さデータＦＨにおける計測点ｐの位置情報（ｘ，ｙ，ｚ）から３次元グリッドの位置情報としてのインデックス（ｉ，ｊ，ｋ）を求め、このインデックスに基づいて３次元グリッドのセルを参照する。
インデックス（ｉ，ｊ，ｋ）を求める処理において、本実施例では、各計測点のノイズの影響を低減するため、高さ情報ｈとして、計測点ｐの実計測高さではなく、計測点ｐが属する平面の高さ（平均化された高さ；plane.m_z）を利用している。勿論、計測点ｐの高さをそのまま利用してもよい。

続いて、グリッドマップ生成部３３は、計測点ｐに対応する高さ情報ｈを更新するか否かを決定する（ステップＳＴ２６）。ここでは、計測点ｐに対応する３次元グリッドの占有確率が障害物が存在することを示す閾値ｔｈよりも大きい場合（ステップＳＴ２６；ｙｅｓ）、フロア高さデータＦＨの対応するセル（ｉ，ｊ）を高さ情報ｈで更新する（ステップＳＴ２７）。

フロア高さデータＦＨにおけるセル（ｉ，ｊ）の更新則としては、幾つかの方法があげられる。例えば、上書き更新とする場合、計測された最大値を保持する場合等があげられる。上書き更新とする場合には、セル（ｉ，ｊ）の高さを取得されたｈによって常に上書きする。また、最大値を保持する場合には、セル（ｉ，ｊ）の高さが今までの高さ情報ｈより低いとき、又はセル（ｉ，ｊ）に無効フラグが立っている場合にのみ、新しい値ｈで更新する。このほかに、今までの高さ情報ｈと新しい値との平均をとってもよい。

図１９は、この一連の処理によって作成された環境地図を表している。上述したように、フロア高さデータＦＨを３次元グリッドマップデータＧＭによって補完する。環境地図は、２次元グリッドのセルに対して高さ情報ｈ又は障害物（obs）か否かがあてはめられたものになっている。

グリッドマップ生成部３３が生成するフロア高さデータＦＨは、例えば、２次元座標（ｘ、ｙ）の各々について、そのフロア高さＦｌｏｏｒ（ｘ，ｙ）を示している。

［環境クラス分類部３４］
環境クラス分類部３４は、グリッドマップ生成部３３から３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを入力し、これらを基に、２次元セル（ｘ，ｙ）に対応する位置を、例えば、障害物、でこぼこ、トンネル、未知、フロア、階段、境界部のいずれかに分類した環境クラス分類データである変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を生成し、これをパス決定部３５に出力する。

環境クラス分類部３４は、先ず、フロア高さデータＦＨを基に、２次元座標（ｘ、ｙ）のセル（ｘ、ｙ）のフロア高さＦｌｏｏｒ（ｘ，ｙ）を取得し、これをｈｆに代入する。
なお、フロア高さＦｌｏｏｒ（ｘ，ｙ）＝−∞（ｉｎｖ）の場合には、フロアが無い、あるいはフロアではないことを示している。
次に、環境クラス分類部３４は、２次元座標（ｘ、ｙ）上の「ｚ＞ｈｆ」を満たす３次元座標（ｘ、ｙ、ｚ）の占有確率ＯＧ（ｘ、ｙ、ｚ）を、３次元グリッドマップデータＧＭから取得する。
そして、環境クラス分類部３４は、上記セル（ｘ、ｙ）上記取得した占有確率ＯＧ（ｘ、ｙ、ｚ）のうち、しきい値Ｐｏｃｃを超えるものを特定し、そのうち最も高いｚを障害物の高さを示すＯｂｓｔａｃｌｅ（ｘ，ｙ，ｈｆ）とする。
そして、上記セル（ｘ、ｙ）の「ｚ＞ｈｆ」の全てのｚにおいて、占有確率ＯＧ（ｘ、ｙ、ｚ）が所定しきい値Ｐｏｃｃ（例えば、０．８）を超えていない場合に、Ｏｂｓｔａｃｌｅ（ｘ，ｙ，ｈｆ）＝−∞（障害ではない）とする。

また、環境クラス分類部３４は、上記セル（ｘ、ｙ）の上記取得した占有確率ＯＧ（ｘ、ｙ、ｚ）のうち、しきい値Ｐｏｃｃを超えるものを特定し、そのうち最も低いｚを天井の高さを示すＣｅｉｌｉｎｇ（ｘ，ｙ，ｈｆ）とする。
そして、上記セル（ｘ、ｙ）の「ｚ＞ｈｆ」の全てのｚにおいて、占有確率ＯＧ（ｘ、ｙ、ｚ）が所定しきい値Ｐｏｃｃ（例えば、０．８）を超えていない場合に、Ｃｅｉｌｉｎｇ（ｘ，ｙ，ｈｆ）＝−∞（天井ではない）とする。

また、環境クラス分類部３４は、フロア高さデータＦＨが示す上記セル（ｘ、ｙ）のフロア高さＦｌｏｏｒ（ｘ，ｙ）と、当該セルに隣接する２次元座標（ｘ’、ｙ’）の隣接セル（ｘ’、ｙ’）のフロア高さＦｌｏｏｒ（ｘ’，ｙ’）との差分の絶対値の最大値をΔｈ（ｘ、ｙ）として算出する。

図２０は、環境クラス分類部３４による各セル（ｘ、ｙ）の環境クラス分類処理を説明するためのフローチャートである。
先ず、環境クラス分類部３４は、上述したように、グリッドマップ生成部３３から入力した３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを基に、各セル（ｘ、ｙ）の上記フロア高さＦｌｏｏｒ（ｘ，ｙ）、上記Ｏｂｓｔａｃｌｅ（ｘ，ｙ，ｈｆ）、Ｃｅｉｌｉｎｇ（ｘ，ｙ，ｈｆ）をそれぞれ取得あるいは生成し、それぞれを変数ｆ，ｏ，ｃに代入する（ステップＳＴ４１）。

次に、環境クラス分類部３４は、「変数ｏ＞ｆ」の条件を満たすか否かを判断し、満たすと判断するとステップＳＴ４３に進み、そうでない場合にはステップＳＴ５１に進む（ステップＳＴ４２）。
ここで、条件「変数ｏ＞ｆ」を満たさない場合（ステップＳＴ５１に進む場合）は、例えば、「Ｏｂｓｔａｃｌｅ（ｘ，ｙ，ｈｆ）＝−∞（障害ではない）」と設定された場合である。

次に、環境クラス分類部３４は、「変数ｆがｉｎｖ（無効）ではない」且つ「ｃ＞ｆ＋ｄ＿ｔｕｎｎｅｌ」の条件を満たすか否かを判断し（ステップＳＴ４３）、満たすと判断すると、セル（ｘ、ｙ）はトンネルであると判定し（ステップＳＴ４４）、そうでない場合にはステップＳＴ４５に進む。
ここで、「ｄ＿ｔｕｎｎｅｌ」は、ロボット１が、天井下において這いながら走行可能な最小高さを示している。すなわち、トンネル内の最小高さを示している。

そして、環境クラス分類部３４は、「変数ｏが所定のしきい値を超えていない」という条件を満たすか否かを判断し、当該条件を満たすと判断すると、セル（ｘ，ｙ）は荒れた面（ｃｌｕｔｔｅｒ）であると判定する（ステップＳＴ４７）。
ここで、「変数ｏが所定のしきい値を超えていない」という条件は、例えば、変数ｏが示す障害物の高さが、ロボット１が乗り越えられた程度に小さい場合に満たされる。
一方、環境クラス分類部３４は、「変数ｏが所定のしきい値を超えていない」という条件を満たさないと判断すると、セル（ｘ，ｙ）は障害物（ｏｂｓｔａｃｌｅ）であると判定する（ステップＳＴ４６）。
なお、上記荒れた面は、ロボット１が簡単に乗り越えることができる程度の障害しかない面を意味している。
また、上記障害物は、ロボット１が走行できない程度に大きい障害物を意味している。

環境クラス分類部３４は、ステップＳＴ５１において、変数ｆが無効（ｉｎｖ）であるか否かを判断し（ステップＳＴ５１）、無効であると判断するとセル（ｘ、ｙ）は未知（ｕｎｋｎｏｗｎ）であると判断し（ステップＳＴ５２）、無効ではないと判断するとステップＳＴ５３に進む。

環境クラス分類部３４は、ステップＳＴ５３において、前述したように、セル（ｘ、ｙ）のフロア高さＦｌｏｏｒ（ｘ，ｙ）と、当該セルに隣接する２次元座標（ｘ’、ｙ’）の隣接セル（ｘ’、ｙ’）のフロア高さＦｌｏｏｒ（ｘ’，ｙ’）との差分の絶対値の最大値をΔｈ（ｘ、ｙ）として算出する。そして、環境クラス分類部３４は、変数ｄに上記算出したΔｈ（ｘ、ｙ）を代入する（ステップＳＴ５２）。
次に、環境クラス分類部３４は、条件「ｄ＜ｄ＿ｓｔｅｐ」を満たすか否かを判断し、満たすと判断すると、セル（ｘ、ｙ）はフロア（ｆｌｏｏｒ）であると判定し（ステップＳＴ５５）、そうでない場合にはステップＳＴ５６に進む。
ここで、「ｄ＿ｓｔｅｐ」は、ロボット１が通常歩行できる段差の最大高さを示し、予め設定される。
また、フロアは、ロボット１が容易に通常歩行できる平面である。

環境クラス分類部３４は、ステップＳＴ５４において、条件「ｄ＜ｄ＿ｓｔａｉｒｓ」を満たすか否かを判断し、満たすと判断すると、セル（ｘ、ｙ）は階段（ｓｔａｉｒｓ）であると判定し（ステップＳＴ５７）、そうでない場合にはセル（ｘ，ｙ）は境界部（ｂｏｒｄｅｒ）であると判定する（ステップＳＴ５８）。
ここで、「ｄ＿ｓｔａｉｒｓ」は、ロボット１が階段上り歩行可能な段差の高さの最大値を示し、予め設定される。

環境クラス分類部３４による上述した処理により、３次元座標（ｘ，ｙ，ｚ）で規定されるセルの状態が、例えば、図２１に示すように分類（判定）される。
環境クラス分類部３４は、上述したように、全セル（ｘ，ｙ）について判定した状態の結果を変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）に格納し、これをパス決定部３６に出力する。
なお、環境クラス分類部３４は、変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）をパス決定部３６に出力する前に、所定のメモリ（例えば、図４に示すＲＡＲ１３１２や外部記憶装置３１４）に書き込み、それを読み出してパス決定部３６に出力する。

なお、上述した例では、セル（ｘ、ｙ）の状態として、ｆｌｏｏｒ、ｏｂｓｔａｃｌｅ，ｕｎｋｎｏｗｎ，ｓｔａｉｒｓ，ｂｏｒｄｅｒ，ｔｕｎｎｅｌ，ｃｌｕｔｔｅｒを例示したが、それ以外にも、環境クラス分類部３４は、障害物が接近した狭い領域を意味するｎａｒｒｏｗ、傾斜面を意味するｒａｍｐ、人間等を意味するｈｕｍａｎ、階段を意味するｌａｄｄｅｒ、通常のドアを意味するｄｏｏｒ、エレベータを意味するｅｌｅｖａｔｏｒ、エスカレータを意味するｅｓｃａｌａｔｏｒなどのその他の状態を判定してもよい。

［動作機能部］
ＣＰＵ１５は、例えば、デフォルトとして、通常走行処理部３５＿１、階段走行処理部３５＿２、注意走行処理部３５＿３、這動作処理部３５＿４、ステップオーバ処理部３５＿５、よじ登り／ジャンプ処理部３５＿６を、例えば、プログラムを実行して実現する。
上記処理部３５＿１〜３５＿６の各々には、例えば、（ｎａｍｅ、ｉｎｆｏ，ｔｙｐｅｓ，ｃｏｓｔ，ｓｅｎｓｏｒｓ，ｅｘｃｕｔｅｒ）という属性データが割り当てられている。
ｎａｍｅは、その処理部（モジュール）の名前を示す。
ｉｎｆｏは、その処理部の機能の記述、例えば、プログラムコードを示す。
ｔｙｐｅｓは、その処理部によって対応可能なセルの状態を示す。
ｃｏｓｔは、ロボット１が移動する最適パス（経路）を決定する際に、その処理部の処理に割り当てるコストを示す。当該コストは、その処理部の制御による動作をロボット１が行った場合に、その位置を通過するのに要する時間に比例した値を持つ。
ｓｅｎｓｏｒｓは、その処理部の処理に入力として必要なセンサを示す。
ｅｘｃｕｔｅは、その処理部の処理を実行するシステム（ハードウェア）を示す。

通常走行処理部３５＿１は、例えば、ロボット１が平坦なフロアを２足歩行する一連の動作を動き制御部３７に指示する処理を行う。
階段走行処理部３５＿２は、例えば、ロボット１が階段を２足歩行する一連の動作を動き制御部３７に指示する処理を行う。
注意走行処理部３５＿３は、例えば、ロボット１がセンサ群１７からの検出信号を基に表面状態を確認しながら、ゆっくりとした動作で慎重に歩行する一連の動作を動き制御部３７に指示する処理を行う。
這動作処理部３５＿４は、例えば、高い障害物がある場合に、ロボット１が腰を屈めて頭を避けて歩いたり、膝をついて歩いたり、四足で這いつくばって歩く一連の這動作を制御する。
ステップオーバ処理部３５＿５は、ロボット１が小さい障害物を跨いで移動する一連の動作を動き制御部３７に指示する。
よじ登り／ジャンプ処理部３５＿６は、ロボット１が障害物を手を利用してよじ登ったり、境界部をジャンプしたりする一連の動作を動き制御部３７に指示する。

ＣＰＵ１５が実現する処理部としては、上述したものの他にも、横方向にゆっくり歩く動作（ｓｉｄｅｗａｙｓ）を規定する処理部、人間と会話をする動作（ｄｉａｌｏｇ）を規定する処理部、障害物を移動する動作（ｍａｎｕｐｕｌａｔｅ）を規定する処理部、ドアを開ける動作（ｏｐｅｎ ｄｏｏｒ）を規定する処理部、傾斜面を移動する動作（ｓｌｉｄｅ）を規定する処理部、エレベータを使う動作（ｕｓｅ ｅｌｅｖａｔｏｒ）を規定する処理部、エスカレータを使う動作（ｕｓｅ ｅｓｃａｌａｔｏｒ）を規定する処理部などがある。

ＣＰＵ１５は、上述した処理部のうち、何れかを予めデフォルトとして備え、それらはユーザからの指示などにより削除可能である。
また、ＣＰＵ１５は、デフォルトとして備えた処理部に加えて、新たな処理部を追加可能である。

［パス決定部３６］
パス決定部３６は、環境クラス分類部３４から入力した、全セル（ｘ，ｙ）について判定した状態の結果を示す変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）と、上述した利用可能な動作機能部のリストとに基づいて、ロボット１が現在地から目的地に移動する最適なパスを決定する。
また、パス決定部３６は、利用可能な動作機能部のリストを常時更新する。
また、パス決定部３６は、上記決定したパスとロボット１の移動状況に基づいて、適切な処理部を順次選択し、ロボット１の動作を制御させる。

先ず、パス決定部３６による最適パスの決定処理を説明する。
図２２は、図５に示すパス決定部３６による最適パスの決定処理を説明するためのフローチャートである。
パス決定部３６は、利用可能機能リストを参照して、利用可能な処理部を特定する（ステップＳＴ６１）。
本実施形態では、一例として、図５に示すように、利用可能な処理部が、通常走行処理部３５＿１、階段走行処理部３５＿２、注意走行処理部３５＿３、這動作処理部３５＿４、ステップオーバ処理部３５＿５、並びによじ登り／ジャンプ処理部３５＿６である場合を説明する。

次に、パス決定部３６は、環境クラス分類部３４から入力した変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を基に、ロボット１の現在位置から目標位置に達する複数のパス（経路）を特定する（ステップＳＴ６２）。

次に、パス決定部３６は、ステップＳＴ６２で特定した複数のパスの各々について、そのパス上の座標（ｘ、ｙ）と、その変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）が示す状態と、その状態を移動するロボット１の機能部に割り当てられた上記コストｃｏｓｔとを基に、コストを算出する（ステップＳＴ６３）。
次に、パス決定部３６は、ステップＳＴ６２で特定した複数のパスのうち、ステップＳＴ６３で算出したコストが最小のパスを決定する（ステップＳＴ６４）。
具体的には、パス決定部３６は、上記複数のパスの各々について、当該パス上の座標（ｘ，ｙ）に対応する上記コストｃｏｓｔの総和を計算し、その総和が最小のパスを決定する。

以下、パス決定部３６による利用可能機能リストの更新処理を説明する。
図２３は、図５に示すパス決定部３６による利用可能機能リスト更新処理を説明するためのフローチャートである。
パス決定部３６は、ロボット１の動作を規定する新たな処理部（機能）が追加されたか否かを判断し（ステップＳＴ６１）、追加されたと判断するとステップＳＴ６２に進み、そうでない場合にはステップＳＴ６３に進む。
パス決定部３６は、利用可能機能リストに、新たに追加された機能（処理部）のエントリを追加する（ステップＳＴ６２）。
ここで、ロボット１のＣＰＵ１５への上記機能の追加は、プログラムモジュールの追加や、あるいは専用ハードウェアの追加によって実現される。

パス決定部３６は、利用可能機能リストのエントリとなっている機能が削除されたか、あるいは、故障したかを判断し（ステップＳＴ６３）、そうであると判断するとステップＳＴ６４に進み、そうではないと判断するとステップＳＴ６１に戻る。
パス決定部３６は、利用可能機能リストから、上記削除あるいは故障した機能のエントリを削除する（ステップＳＴ６４）。

以下、パス決定部３６による処理部の選択処理を説明する。
図２４および図２５は、図５に示すパス決定部３６による処理部の選択処理を説明するためのフローチャートである。
パス決定部３６は、動き制御部３７の制御の進行状況、ＣＣＤカメラ１０Ｒ，１０Ｌの撮像結果、並びにセンサ群２７からの検出信号の少なくとも一つに基づいて、ロボット１が次に移動するセル（ｘ、ｙ）を特定する（ステップＳＴ７１）。
次に、パス決定部３６は、ステップＳＴ７１で特定したセル（ｘ、ｙ）の変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を変数ｔに代入する（ステップＳＴ７２）。

次に、パス決定部３６は、変数ｔがｆｌｏｏｒを示すか否かを判断し（ステップＳＴ７３）、ｆｌｏｏｒを示す場合に通常走行処理部３５＿１に対して動作指示を出力し（ステップＳＴ７４）、そうでない場合にステップＳＴ７５に進む。
ステップＳＴ７４において、動き制御部３７が、通常走行処理部３５＿１からの指示に基づいて、ロボット１に通常走行動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、変数ｔがｓｔａｉｒｓを示すか否かを判断し（ステップＳＴ７５）、ｓｔａｉｒｓを示す場合に階段走行処理部３５＿２に対して動作指示を出力し（ステップＳＴ７６）、そうでない場合にステップＳＴ７７に進む。
ステップＳＴ７６において、動き制御部３７が、階段走行処理部３５＿２からの指示に基づいて、ロボット１に階段走行動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、変数ｔがｕｎｋｎｏｗｎを示すか否かを判断し（ステップＳＴ７７）、ｕｎｋｎｏｗｎを示す場合に注意走行処理部３５＿３に対して動作指示を出力し（ステップＳＴ７８）、そうでない場合にステップＳＴ７９に進む。
ステップＳＴ７８において、動き制御部３７が、注意走行処理部３５＿３からの指示に基づいて、ロボット１に注意走行動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、変数ｔがｔｕｎｎｅｌを示すか否かを判断し（ステップＳＴ７９）、ｔｕｎｎｅｌを示す場合に這動作処理部３５＿４に対して動作指示を出力し（ステップＳＴ８０）、そうでない場合にステップＳＴ８１に進む。
ステップＳＴ８０において、動き制御部３７が、這動作処理部３５＿４からの指示に基づいて、ロボット１に這動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、変数ｔがｃｌｕｔｔｅｒを示すか否かを判断し（ステップＳＴ８１）、ｃｌｕｔｔｅｒを示す場合にステップオーバ処理部３５＿５に対して動作指示を出力し（ステップＳＴ８２）、そうでない場合にステップＳＴ８３に進む。
ステップＳＴ８２において、動き制御部３７が、ステップオーバ処理部３５＿５からの指示に基づいて、ロボット１にステップオーバ動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、変数ｔがｂｏｒｄｅｒを示すか否かを判断し（ステップＳＴ８３）、ｂｏｒｄｅｒを示す場合によじ登り／ジャンプ処理部３５＿６に対して動作指示を出力し（ステップＳＴ８４）、そうでない場合にステップＳＴ８５に進む。
ステップＳＴ８４において、動き制御部３７が、よじ登り／ジャンプ処理部３５＿６からの指示に基づいて、ロボット１によじ登り動作あるいはジャンプ動作を行わせるようにアクチュエータ群１６を制御する。

次に、パス決定部３６は、動き制御部３７の制御の進行状況、ＣＣＤカメラ１０Ｒ，１０Ｌの撮像結果、並びにセンサ群２７からの検出信号の少なくとも一つに基づいて、目的地に到達したか否かを判断し（ステップＳＴ８５）、目的地に到達したと判断すると処理を終了し、そうでない場合にはステップＳＴ７１に戻る。

パス決定部３６は、例えば、図２６に示すように、領域ＡＲ１，ＡＲ３，ＡＲ５はｆｌｏｏｒであり通常走行処理部３５＿１に制御を行わせ、領域ＡＲ２はｓｔａｉｒｓであり階段走行処理部３５＿２に制御を行わせ、領域ＡＲ４は境界部でありよじ登り／ジャンプ処理部３５＿６に制御を行わせ、領域ＡＲ６はｕｎｋｎｏｗｎであり注意走行処理部３５＿３に制御を行わせる。
また、パス決定部３６は、例えば、図２７に示すように、ロボット１の現在位置（Ｒｏｂｏｔ）から目的地（Ｇｏａｌ）までの最適パスを決定する。

［動き制御部３７］
動き制御部３７は、通常走行処理部３５＿１、階段走行処理部３５＿２、注意走行処理部３５＿３、這動作処理部３５＿４、ステップオーバ処理部３５＿５、よじ登り／ジャンプ処理部３５＿６などからの指示に基づいて、アクチュエータ群１６を駆動し、所望の動作をロボット１に実行させる。

［ロボット１の全体動作］
以下、図１に示すロボット１の全体動作例を説明する。
ＣＣＤカメラ１０Ｒ，１０Ｌが、それぞれ撮像結果に応じた右画像データＳ１０Ｒおよび左画像データＳ１０Ｌをステレオ画像処理装置１２に出力する。
そして、ステレオ画像処理装置１２が、ＣＣＤカメラ１０Ｒ，１０Ｌから入力した右画像データＳ１０Ｒと左画像データＳ１０Ｌの視差情報（disparity data）（距離情報）を計算し、カラー画像データ（ＹＵＶ：輝度Ｙ、ＵＶ色差）Ｓ１２ａ及び視差画像データ（ＹＤＲ：輝度Ｙ、視差Ｄ、信頼度Ｒ）Ｓ１２ｂをフレーム毎に左右交互に算出し、これをＣＰＵ１５（図５に示す距離データ生成部３１）に出力する。

次に、距離データ生成部３１が、ステレオ画像処理装置１２から入力したカラー画像データＳ１２ａ及び視差画像データＳ１２ｂを基に、画像（環境）内の対象物についての３次元の距離データ（ステレオデータ）Ｓ３１を生成し、これを平面検出部３２およびグリッドマップ生成部３３に出力する。
次に、平面検出部３２は、距離データ生成部３１から入力した距離データＳ３１から環境内に存在する複数の平面を検出し、それを示す平面データＳ３２をグリッドマップ生成部３３に出力する。
次に、グリッドマップ生成部３３が、距離データ生成部３１から入力した距離データＳ３１と平面検出部３２から入力した上記平面検出データＳ３２とに基づいて、３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを生成し、これらを環境クラス分類部３４に出力する。

次に、環境クラス分類部３４が、グリッドマップ生成部３３から３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを入力し、これらを基に、２次元セル（ｘ，ｙ）に対応する位置を、例えば、障害物、でこぼこ、トンネル、未知、フロア、階段、境界部のいずれかに分類した環境クラス分類データである変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を生成し、これをパス決定部３５に出力する。
次に、パス決定部３６が、環境クラス分類部３４から入力した、全セル（ｘ，ｙ）について判定した状態の結果を示す変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）と、上述した利用可能な動作機能部のリストとに基づいて、ロボット１が現在地から目的地に移動する最適なパスを決定する。
また、パス決定部３６は、利用可能な動作機能部のリストを常時更新する。
また、パス決定部３６は、上記決定したパスとロボット１の移動状況に基づいて、適切な処理部３５＿１〜３５＿６を順次選択する。
そして、選択された処理部３５＿１〜３５＿６が動き制御部３７に指示を出す。
次に、動き制御部３７が、上記選択された処理部３５＿１〜３５＿６からの指示に基づいて、アクチュエータ群１６を駆動し、所望の動作をロボット１に実行させる。

以上説明したように、ロボット１では、図５等に示すように、ＣＰＵ１５が機能として利用可能な処理部３５＿１〜３５＿６等を特定し（図２２に示すステップＳＴ６１）、その利用な処理部を基に、目標位置までの複数のパスを動的に特定する（ステップＳＴ６２）。
これにより、例えば、処理部３５＿１〜３５＿６あるいはそれによって制御されるアクチュエータ群１６などの機械部位が故障した場合に、その故障した機能に対応した処理部による制御を利用しないで目標位置に達するパスを選択することができる。
また、新たな処理部を追加して利用可能にした場合に、その追加した処理部の機能を利用してロボット１が走行可能なパスを選択枠に入れることができる。
また、ロボット１は、それが位置する環境に応じて利用可能な処理部を動的に変更して最適パスを決定することも可能である。
また、ロボット１では、上記複数のパスの各々について、そのパス上の２次元座標の変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）と、それに対応付けられたコスト（時間コスト）とを基に、コストの和を最小にするパスを選択するため、最短時間で到達できるパスを高い信頼性で選択できる。

また、ロボット１では、環境クラス分類部３４において、３次元グリッドマップデータＧＭおよびフロア高さデータＦＨを基に、２次元座標（ｘ、ｙ）の各々についてその状態を分類した変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を生成する。
そして、パス決定部３６において、ロボット１の移動過程において、ロボット１の２次元位置（ｘ、ｙ）に応じて、それに対応した上記状態を特定し、その状態に予め対応付けられた処理部３５＿１〜３５＿６を選択してロボット１の動作を駆動させる。
ロボット１では、上述したように、予め２次元座標（ｘ、ｙ）の状態を分類した変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を生成し、これを図４に示すＲＡＭ３１２などに記憶する。
これにより、ロボット１の動作時に、パス決定部３６は、環境クラス分類部３４から変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を読み出すだけで、ロボット１が次に移動する位置の状態を取得でき、それを基にロボット１の次の動作を短時間で決定できると共に、処理負荷を従来に比べて軽減できる。
また、ロボット１では、次元座標（ｘ、ｙ）の状態を分類した変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）をＲＡＭ３１２などのメモリに記憶していれば十分であり、変数ｅｎｖ＿ｔｙｐｅ（ｘ，ｙ）を生成後に、グリッドマップデータＧＭおよびフロア高さデータＦＨを保持する必要が無く、メモリを効率的に使用できる。

本発明は上述した実施形態には限定されない。
すなわち、当業者は、本発明の技術的範囲またはその均等の範囲内において、上述した実施形態の構成要素に関し、様々な変更、コンビネーション、サブコンビネーション、並びに代替を行ってもよい。
例えば、上述した実施形態では、本発明の移動体として、２足歩行のロボット１を例示したが、移動体として、例えば、車輪による移動体、あるいは１足や３足以上のロボットを用いてもよい。

図１は、本発明の実施形態に係わるロボットの機能ブロック図である。 図２は、本発明の実施形態に係わるロボットの外観図である。 図３は、本発明の実施形態のロボットが具備する関節自由度構成を模式的に示す図である。 図４は、本発明の実施形態に係わるロボットの制御システム構成を示す模式図である。 図５は、図１に示すＣＰＵの機能ブロック図である。 図６は、本発明の実施形態に係わるロボットによって認識される実空間を説明する模式図である。 図７は、本発明の実施形態に係わるロボットが外界を撮影している様子を示す模式図である。 図８は、図５に示す平面検出部の機能ブロック図である。 図９は、図８に示す平面検出部が行う線分拡張法による平面検出方法を説明する図である。 図１０は、線分拡張法による平面検出処理を示すフローチャートである。 図１１は、階段を示す模式図であって、（ａ）は階段を正面からみた図、（ｂ）は階段を側面からみた図、（ｃ）は階段を斜めからみた図である。 図１２は、階段のほかの例を示す模式図であって、（ａ）は階段を正面からみた図、（ｂ）は階段を側面からみた図、（ｃ）は階段を斜めからみた図である。 図１３は、３次元グリッドが観測結果に応じて更新される様子をｙ軸方向からみた模式図の側面図である。 図１４は、テクスチャを付与する手段を有しているロボットを説明するための図である。 図１５は、図６に示す実空間と同じ領域を表す３次元グリッドを説明する模式図である。 図１６は、図６に示す空間の高さ地図を示す模式図である。 図１７は、図５に示すグリッドマップ生成部によるフロア高さデータＦＨの生成処理を説明するフローチャートである。 図１８は、図５に示すグリッドマップ生成部によるフロア高さデータＦＨの更新処理を説明するための図である。 図１９は、図５に示す環境クラス分類部が生成したデータを説明するための図である。 図２０は、図５に示す環境クラス分類部による各セル（ｘ、ｙ）の環境クラス分類処理を説明するためのフローチャートである。 図２１は、図５に示す環境クラス分類部の処理を説明するための図である。 図２２は、図５に示すパス決定部による最適パスの決定処理を説明するためのフローチャートである。 図２３は、図５に示すパス決定部による利用可能機能リスト更新処理を説明するためのフローチャートである。 図２４は、図５に示すパス決定部による処理部の選択処理を説明するためのフローチャートである。 図２５は、図５に示すパス決定部による処理部の選択処理を説明するための図２４の続きのフローチャートである。 図２６は、図５に示すパス決定部の処理の一例を説明するための図である。 図２７は、図５に示すパス決定部の処理の一例を説明するための図である。

符号の説明

１０Ｌ，１０Ｒ…ＣＣＤカメラ、１２…ステレオ画像処理装置、１５…ＣＰＵ、１６…アクチュエータ群、１７…センサ群、３１…距離データ生成部、３２…平面検出部、３３…グリッドマップ生成部、３４…環境クラス分類部、３５＿１…通常走行処理部、３５＿２…階段走行処理部、３５＿３…注意走行処理部、３５＿４…這動作処理部、３５＿５…ステップオーバ処理部、３５＿６…よじ登り／ジャンプ処理部、３６…パス決定部、３７…動き制御部

Claims (11)

1. 移動体が目標位置まで移動する動作を制御する移動体制御方法であって、
   前記移動体の現在位置から前記目標位置までのパスの状態を検出する第１の工程と、
   前記移動体が実行可能な複数の移動動作を特定する第２の工程と、
   前記第１の工程で検出したパスの状態に基づいて、前記第２の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように制御する第３の工程と
   を有する移動体制御方法。
2. 前記移動体が実行可能な前記移動動作を追加または削除する第４の工程
   をさらに有し、
   前記第２の工程は、前記第４の工程で追加または削除された後の前記実行可能な移動動作を特定する
   請求項１に記載の移動体制御方法。
3. 前記第３の工程は、
   前記移動体の現在位置から前記目標位置までの複数のパスの各々について、当該パスを前記移動体を走行した場合の走行コストの総和を計算し、当該総和を最小にする前記パスを、前記移動体が移動するパスとして選択する
   請求項１に記載の移動体制御方法。
4. 前記第１の工程は、異なる複数の撮像位置からの撮像対象領域の撮像結果を基に、前記パス上の位置の状態を判定し、
   前記第３の工程は、前記移動体の位置について前記判定した状態に予め対応付けられた移動動作を選択する
   請求項１に記載の移動体制御方法。
5. 前記第１の工程は、
   撮像対象領域内に規定された各３次元グリッドの対象物による占有状態を示す３次元グリッドデータを生成し、
   前記生成した３次元グリッドデータを基に、前記３次元グリッドの２次元位置の各々について、予め決められた複数の状態のうち何れの状態であるかを判定し、
   当該判定した状態に応じて前記パスの状態を検出する
   請求項１に記載の移動体制御方法。
6. 前記第１の工程は、前記２次元位置の各々が、前記移動体が移動できるフロアあるいは階段と、前記移動体が移動できない障害物との何れであるかを判定する
   請求項５に記載の移動体制御方法。
7. 前記移動体は、２つの脚部がある人間型のロボットであり、
   前記複数の種類の移動動作は、通常走行、階段走行、注意走行、這動作、ステップオーバ、よじ登り動作、並びにジャンプ動作のうち何れかである
   請求項１に記載の移動体制御方法。
8. 異なる複数の撮像位置からの撮像対象領域の撮像結果を生成する撮像手段と、
   走行手段と、
   前記撮像手段が生成した前記撮像結果を基に、前記移動体の現在位置から前記目標位置までのパスの状態を検出し、前記移動体が実行可能な複数の移動動作を特定し、前記検出したパスの状態に基づいて、前記第２の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように前記走行手段を制御する制御手段と
   を有する移動体。
9. 前記制御手段は、前記移動体が実行可能な前記移動動作を追加または削除する
   請求項８に記載の移動体。
10. 前記制御手段は、前記移動体の現在位置から前記目標位置までの複数のパスの各々について、当該パスを前記移動体を走行した場合の走行コストの総和を計算し、当該総和を最小にする前記パスを、前記移動体が移動するパスとして選択する
    請求項８に記載の移動体。
11. 前記走行手段は、２つの脚部であり、
    人間型のロボットである
    請求項８に記載の移動体。
    

Images