JP4479372B2

JP4479372B2 - 環境地図作成方法、環境地図作成装置、及び移動型ロボット装置

Info

Publication number: JP4479372B2
Application number: JP2004188868A
Authority: JP
Inventors: グッドマンステフェン; 正樹福地; 浩太郎佐部; ブルースジェイムズ
Original assignee: Sony Corp
Current assignee: Sony Corp
Priority date: 2004-06-25
Filing date: 2004-06-25
Publication date: 2010-06-09
Anticipated expiration: 2024-06-25
Also published as: US20060025888A1; US7912583B2; JP2006011880A

Description

本発明は、環境内の情報を取得し環境地図を作成する環境地図作成方法、その装置、及び脚部などの移動手段を有し自身のおかれた環境内の障害物を認識して移動する移動型ロボット装置に関する。

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

ロボット装置が取得した３次元の距離情報から平面を検出するアルゴリズムについては様々な研究が行われている。検出された平面は、例えば移動式のロボット装置の障害物回避、又は階段昇降動作の用途とすることができる。

一般に、距離情報から平面を検出してロボット装置を移動させる手法として、ロボット装置の周囲の環境を２次元格子（Grid）（以下、２次元グリッドと記す。）で表現し、検出される全ての障害物からの仮想斥力（Repulsive Force）とターゲット（目標）に対する仮想引力（Attractive Force）の線形和によって得られる方向にロボット装置を移動させる方法が提案されている（特許文献１）。

また、取得した距離情報を用いて床面検出することで障害物か否かを推定できるロボット装置において、ロボット装置周辺の環境を所定の大きさグリッドに分割されたロボット中心座標系の地図情報としての環境地図で表現し、地図内の各グリッドに対して障害物の存在確率を保持できるようにし、存在確率がある閾値を超えたグリッドを障害物として扱うことによって、ロボット装置に周辺環境を識別させる手法が提案されている（特許文献２）。また、この特許文献２を発展させた手法として、２次元グリッドを拡張し、環境を水平面に平行な複数の２次元グリッドによって表現する手法もある。異なる高さの平面を複数の２次元グリッドによって表現している。

更には、ステレオビジョンを用いて周囲の環境の３次元情報を計測し、周囲の環境を３次元格子で表現する技術もある（例えば、非特許文献１参照）。また、ステレオビジョンによって得られる３次元情報から２次元グリッドの各グリッドに対して高さ情報だけをアナログ値で割り当てて構築した２．５次元高さ地図（2.5D Height Map）によって、２次元グリッドの１マスに高さ情報をあてはめることで環境を表現する手法もある。（例えば、非特許文献２参照）
米国特許第５００６９８８号公報特開２００３−２６９９３７号公報 H.Moravec.「確率的容積検出による強固な探索法（Robust Navigation by Probabilistic Volumetric Sensing）」,DARPA MARS program research progress,2002. Kagamiら（S.Kagami,K.Nishiwaki,J.J.Kuffner,K.Okada,M.Inaba,H.Inoue.）「人間型ロボットの移動のための視覚に基づいた２．５次元地形モデリング（Vision-based 2.5D terrain modeling for humanoid locomotion.）」,Int.Conf.on Robotics and Automation (ICRA),2003.

特許文献１及び特許文献２のような技術は、周囲の環境を２次元地図で表すため、移動体（移動型ロボット）の性能が向上し、段差、傾斜等、移動面の状態変化にある程度対応できるようになってくると不十分であり、垂直方向の情報が必要となってくる。

しかし、垂直方向の解像度を向上するために、高さ方向にもグリッドを積み上げた３次元グリッドを解析することによって環境を精度よく表現しようとした非特許文献３に代表される技術では、周囲の環境を２次元グリッドとして扱う場合に比べて観測が必要なグリッド数が多くなる。

例えば、４メートル四方、高さ１メートルの空間を、水平解像度４センチ及び垂直解像度１センチを１グリッド単位（場合によってはセルと記す。）とした３次元グリッドで表現しようとした場合、１００×１００×１００＝１，０００，０００個のセルが必要になる。１，０００，０００個のセルに対して障害物存在確率を統計的に処理する計算量、必要なメモリ容量等が増大するため、特に、搭載可能な情報処理装置、情報取得のための検出装置等に制限がある２足歩行ロボット（Humanoid Robot）に対してこの技術を適用することは困難である。そのうえ、２足歩行ロボットは、垂直方向に対して４ｃｍ以下の精度で解析することができないのでは、階段登行、段差越えといった動作の際に不都合なため、実際には、垂直方向に対して４ｍｍ程度の解像度が要求される。そうなると、グリッド数は、更に１０倍となり、精度の高い環境認識を行うことによる計算量とメモリ容量の問題が回避できない。

３次元グリッドを用いる場合に比べて情報量を削減した２．５次元高さ地図（2.5D Height Map）を使用した非特許文献２の技術は、高さ方向の解像度がグリッド毎にアナログ値として保持されるために、３次元グリッドの場合よりも少ないデータ量で精度の高い表現が可能となる点では有益である。

ところが、３次元グリッドに比べて情報量が削減されていることに起因する問題が生じる場合がある。問題点の１つとして、２．５次元高さ地図では、以前に観測した平面が次の観測時になくなっていた場合、環境認識が不十分になることがある。図３６及び図３７を用いて、２．５次元高さ地図によって環境を表現する移動型ロボットが２．５次元高さ地図を更新できない場合について説明する。

図３６には、移動型ロボット４００の前方に平面Ａ、平面Ｂがあるような環境の断面が示されている。移動型ロボット４００は、ステレオビジョン４０１によって、視野領域ＳＳにある平面Ａと平面Ｂとを観測し２．５次元高さ地図を作成する。ここでは説明のために、作成される２．５次元高さ地図を実環境に対応させて、図示しない移動型ロボット装置の足底踏面に基準面をとって、グリッドＧ４１０として示す。グリッドＧ４１０の１つのセルＳは、移動型ロボット周辺の環境を所定の解像度で分割表現したときの格子１つに対応する。一例として、４メートル四方の空間を水平解像度４センチで分割したとする。説明の便宜上、視野領域端に対応するセルから順番にＳ_１、Ｓ_２、・・・、Ｓ_ｎと記す。

移動型ロボットは、決められた時間間隔でステレオビジョン４０１によって環境を観測し、その都度、グリッドＧ４１０を更新する。セルＳ_１〜Ｓ_８には、高さ情報として平面Ａの高さ１００ｍｍが代入される。このとき、図３７に示すように、平面Ａが移動してなくなったとする。移動型ロボット４００は、次の観測で平面Ａに隠れていた平面Ｂを検出する。平面Ｂに含まれるセルＳ_４〜Ｓ_１５に対しては、高さ情報が更新され、平面Ｂの高さ５０ｍｍが代入されるが、平面Ａとして認識され平面Ｂに含まれないセルＳ_１〜Ｓ_３は、図３７の状況では、移動型ロボットからは見えない（観測されない）ため、依然として平面Ａの高さ情報１００ｍｍを保持し続けることになる。

図３６及び図３７に示す例は、環境内の平面（障害物）の移動、消失等によって生じる特殊な環境変化であるが、ステレオビジョン４０１、或いはステレオビジョン４０１の観測データから平面検出を行う平面検出器等の観測ノイズによってもしばしば発生することがある。例えば、平面検出器が、実際には存在しない平面又は障害物を検出したかのような誤検出信号に近い観測ノイズを送った場合などに、実際には存在しない障害物がグリッドＧ４１０上の更新できない領域に残留してしまうという問題点があった。

本発明は、上述した従来の実情に鑑みてなされたものであり、観測ノイズに対して高い耐性を備え、且つ高さ方向に対して高い解像度をもって周囲の環境を表現することができる環境認識装置及び環境認識方法、並びに移動型ロボット装置を提供することを目的とする。また、観測ノイズに対して高い耐性を備え、且つ高さ方向に対して高い解像度をもって周囲の環境を表現することができる環境認識方法をコンピュータに実行させる環境認識プログラムを提供することを目的とする。

本発明に係る環境地図作成方法は、外部状態に基づいて３次元地図を作成する環境地図作成方法であって、外部状態検出手段が、外部状態を検出する外部状態検出工程と、３次元地図作成手段が、外部状態検出工程により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成工程と、対基準面高さ地図作成手段が、外部状態検出工程により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成工程と、高さ地図情報変更手段が、３次元地図の情報に基づいて対基準面高さ地図の情報を変更する高さ地図情報変更工程とを有することにより、対基準面高さ地図を３次元地図の情報を用いて補完する。

特に、高さ地図情報変更工程は、対基準面高さ地図における高さ情報を取得し、該高さ情報を３次元地図の高さ座標に対応させ、対応した３次元グリッドの１グリッド単位の占有確率が３次元グリッドにおいて障害物と認識するか否かを決めている閾値より大きいか否かを比較し、閾値より小さい場合、この１グリッド単位は障害物でないと判断し、閾値より大きい場合、この１グリッド単位は障害物であると判断し、対応する対基準面高さ地図の高さ情報を更新する。

また、平面検出工程は、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、線分抽出工程によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、線分抽出工程は、距離データ点の分布に応じて適応的に線分を抽出することにより平面を検出する。

また、本発明に係る環境地図作成装置は、外部状態に基づいて３次元地図を作成する環境地図作成装置であって、外部状態を検出する外部状態検出手段と、外部状態検出手段により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成手段と、外部状態検出手段により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成手段と、３次元地図の情報に基づいて対基準面高さ地図の情報を変更する高さ地図情報変更手段とを有し、対基準面高さ地図を３次元地図の情報を用いて補完する。

また、本発明に係る移動型ロボット装置は、移動手段により移動可能なロボット装置であって、外部状態を検出する外部状態検出手段と、外部状態検出手段により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成手段と、外部状態検出手段により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成手段と、３次元地図の情報に基づいて対基準面高さ地図の情報を変更する高さ地図情報変更手段と、高さ地図情報変更手段によって変更された地図情報を環境地図として自律的に行動経路を決定し移動動作を制御する移動制御手段とを備える。

本発明によれば、外部状態を検出し、検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元障害物地図を作成し、検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成し、更に３次元障害物地図の情報に基づいて、対基準面高さ地図の情報を変更することにより、同程度の解像度をもった３次元格子に比べて演算負荷を低減することができるとともに、観測ノイズに対して高い耐性を備え、且つ高さ方向に対して高い解像度をもって周囲の環境を表現することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。以下では、ステレオビジョンなどにより得られた距離情報（距離データ）から抽出した複数平面によって障害物や段差などといった環境を認識して環境地図として表現する環境地図作成装置と、この環境地図作成装置を移動型ロボット装置に適用した例について説明する。

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

続いて、環境地図作成装置１がＣＣＤカメラ１０Ｌ、１０Ｒにおいて取得したステレオデータに基づいて環境を環境地図として表現するまでの処理を実行するシステムを、図２に示す機能ブロックを用いて説明する。

図２に示すように、環境地図作成装置１は、３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）２と、ステレオビジョンシステム２からステレオデータＤ１が入力され、このステレオデータＤ１から環境内の平面を検出する平面検出器（Plane Segmentation）３と、平面検出器３から出力される平面データＤ２から後述する環境地図を更新し、環境地図データＤ５を出力する環境地図作成部（Update GridMap & HeightMap）４とを備える。

更に、環境地図作成部４は、図３に示すように、検出された周囲の環境に基づいて３次元環境地図（以下、３次元グリッド）を作成する、又はこれを更新する３次元グリッド更新部（Update 3D Grid）４１と、３次元グリッド更新部４１から送られた障害物確率データＤ３に基づいて高さ地図を検証する高さ地図検証部（Verify Height Map）４２と、検証結果に基づいて高さ地図における高さ情報を更新する高さ地図更新部（Update Height Map）４３とを備えている。

３次元グリッド更新部４１によって作成される３次元環境地図（３次元グリッド）は、周囲の環境を水平方向及び垂直方向に所定の大きさの格子（グリッド；Grid）に分割して表したものである。３次元グリッド更新部４１は、ステレオビジョンシステム２からのステレオデータＤ１から、例えば、４メートル四方、高さ１メートルの空間を、水平解像度４センチ及び垂直解像度１センチの直方体空間を１グリッド単位（１セルと記す。）とし、その集合によって３次元環境地図を表現している。３次元グリッド更新部４１は、各セルに物体によって占有されているか否かの確率を付与し、観測データに基づいて各セルの占有確率を変更させている。占有確率に閾値を設け、占有確率が閾値より大きいか否かによって環境の状態を地図として表現している。

本具体例では、高さ方向の解像度がセル分解能に依存しないため、垂直解像度は、１センチよりも粗くできる。また、３次元グリッド更新部４１は、作成した３次元グリッドをステレオデータＤ１及び平面データＤ２によって更新し、高さ地図検証部４２に出力している。

高さ地図検証部４２は、ステレオデータＤ１から２次元の基準面からの高さを表す対基準面高さ地図を作成している。対基準面高さ地図は、検出される平面を２次元の基準面からの高さを表す情報によって区別することで環境を表現した地図であって、３次元グリッドのように垂直方向（Ｚ軸）に平行な面（物体側面）に関する情報をもたないことから、本具体例では、２．５次元高さ地図（2.5D Height Map）と記している。また、本具体例では、２．５次元高さ地図の地図内領域を特定できるように座標系をあてはめ、３次元グリッドと同領域に関する地図とすることにより、３次元グリッド更新部４１で作成される３次元グリッドに対応付けできるようにしている。高さ地図検証部４２は、３次元グリッドのセルの高さ情報と２．５次元高さ地図のセルの高さ情報とを比較して検証する処理を行っている。

高さ地図更新部４３は、３次元地図の情報に基づいて対基準面高さ地図の情報を変更している。すなわち、平面データＤ２と３次元グリッドの障害物確率データＤ３とから必要に応じて２．５次元高さ地図の高さ情報を更新する処理を行っている。

環境地図作成装置１は、まずステレオビジョンシステム２によって外界を観測し、ステレオ視カメラの視差画像によって算出される３次元距離情報であるステレオデータＤ１を画像として出力する。すなわち、人間の両眼に相当する左右２つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。環境地図作成装置１は、ステレオデータＤ１から平面検出器３によって平面を検出することによって環境内に存在する複数の平面を認識することができる。また、環境地図作成装置１は、環境地図作成部４によって平面データＤ２からステレオビジョンシステム２で取得された周辺の環境を水平方向及び垂直方向に所定の大きさの格子（Grid）に分割された環境地図を作成する。環境地図作成装置１は、環境地図をステレオデータＤ１及び平面データＤ２によって更新し、３次元グリッドの障害物存在確率を参照することによって２．５次元高さ地図を作成している。

上述した環境地図作成装置１の各ブロックの機能及び処理は、後段にて詳細に説明する。

ステレオビジョンシステムにより得られた距離情報（距離データ）から抽出した複数平面によって障害物や段差などといった環境を認識して環境地図として表現する環境地図作成装置１を移動型ロボット装置に適用した例について説明する。移動型ロボット装置の具体例として、２足歩行タイプのロボット装置をあげる。このロボット装置は、住環境そのほかの日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボット装置である。ロボット装置は、環境地図作成装置１によって作成された環境地図を利用することによって、行動経路を作成することができる。

図４は、本具体例におけるロボット装置の概観を示す斜視図である。図４に示すように、ロボット装置２０１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されるとともに、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図１に示した環境地図作成装置１をロボット装置に適用した場合、周囲の環境を環境地図として認識するための構成の概略は、図７に示すようになる。すなわち、ロボット装置２０１では、図１に示す構成に加えて、アクチュエータ１６、各種センサ１７を備えている。ロボット装置２０１では、２台のＣＣＤカメラ１０Ｒ，１０Ｌとして画像入力装置２５１が用いられ、思考制御モジュール２００がステレオ画像処理装置１２として機能する。ステレオ画像処理装置１２は、各画像１１Ｒ，１１Ｌの視差情報（距離情報）を計算し、カラー画像１３及び視差画像１４をフレーム毎に左右交互に算出する。

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

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

図８は、ロボット装置２０１が画像入力装置２５１において取得したステレオデータに基づいて環境を環境地図として表現し、環境地図に基づいて行動経路を決定するまでの処理を実行するシステムを示す機能ブロック図である。図８に示すように、ロボット装置２０１は、ステレオビジョンシステム２、平面検出器３、環境地図作成部４に加え、視差情報及び視差画像からなるステレオデータＤ１及びセンサデータＤ６を入力としてステレオデータＤ１が撮像された時刻におけるセンサデータＤ６の関節角を割り出し関節角データＤ７を送出するキネマティックオドメトリ５と、キネマティックオドメトリ５からの関節各データＤ７によって、ロボット中心座標系をカメラ座標系に変換しロボット姿勢データＤ８を送出するロボット姿勢更新部（Update Robot Pose）６と、環境地図作成部４からの環境地図データＤ５と、ロボット姿勢更新部７からのロボット姿勢データからロボット装置２０１の行動経路決定等の動作指令を制御する経路決定部（Path Planner）７とを備えている。

ロボット装置２０１は、まず、ステレオビジョンシステム２によって外界を観測し、両眼の視差によって算出される３次元距離情報としてのステレオデータＤ１を画像として出力する。すなわち、人間の両眼に相当する左右２つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。この距離画像から平面検出器３によって平面を検出することで環境内に存在する複数の平面を認識することができる。更に、環境地図作成部４によって、ロボット装置の周囲の環境地図を作成する。そして、経路決定部７は、環境地図データＤ５及びロボット姿勢データＤ８を用いて環境内におけるロボット装置の移動動作を実現する。このようにロボット装置２０１に環境地図作成装置１を適用することによって、ロボット装置２０１は、自身の視野内にある階段、障害物、段差等の視野内情報を環境地図として認識することができ、環境地図を参照することによって自分の周辺の障害物、階段、段差等を含む環境を把握できるようになり、この認識結果としての環境地図を使用して、階段昇降動作、歩行動作、行動経路探索といった動作が可能になる。

図９には、環境地図作成装置１を備えるロボット装置２０１によって認識される環境の例が示されている。図９に示す実空間Ｓ５００には、ロボット装置２０１が登行可能な平面ＰＬ５０１とＰＬ５０２とを備える階段（Staircase）ＳＴ５０３と、ロボット装置が登行不可能な障害物（Obstacle）ＯＢ５０４，５０５とが示されている。

図１０は、ロボット装置２０１が外界を撮影している様子を示す模式図である。ここでは、床面（基準面）をｘ−ｙ平面とし、高さ方向をｚ軸としたとき、図１０に示すように、画像入力部（ステレオカメラ）を頭部ユニット２０３に有するロボット装置２０１の視野範囲は、ロボット装置２０１の前方の所定範囲となる。ロボット装置２０１は、視野範囲にある環境を認識して環境地図を作成する。

続いて、ロボット装置２０１が図９の実空間Ｓ５００を認識し環境地図を作成する処理について環境地図作成装置１の構成の説明とともに図面を参照して説明する。

ステレオビジョンシステム２は、３次元の距離データ、カラー画像及びステレオカメラによる視差画像などの画像データを取得し、計測された距離情報（ステレオデータＤ１）を平面検出器３に送出する。

平面検出器３は、ステレオビジョン等の距離計測器から出力される距離情報（ステレオデータＤ１）から環境内に存在する複数の平面を検出し、平面データＤ２を出力する。ここでの平面の検出方法としては、線分拡張法、ハフ変換等を利用した公知の平面検出技術を適用することができる。ノイズを含む距離データから階段のように複数平面を検出する代表例として、線分拡張法による平面検出を行うと正確に平面を検出することができる。

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

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

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

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

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

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

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

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

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

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

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

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

図１６乃至図１８における（ａ）は、図１５に示す階段ＳＴ２をステレオビジョンシステムによってそれぞれ前方、側方、斜め前方から撮影した場合の画像を示す模式図、（ｂ）乃至（ｃ）は（ａ）に示す画像から取得した３次元の距離データを示す図である。

図１６（ａ）に示すように、正面から階段ＳＴ２を撮影した場合、３次元の距離データは、図１６（ｂ）乃至図１６（ｃ）のようになる。図１６（ｂ）において、横軸をｙ方向、縦軸をｘ方向とし、ｚ軸方向（高さ方向）の大きさは、ロボット装置２０１の接地面を０とし、高さが高くなるほど白に近づくような濃淡値で示している。すなわち、濃淡（濃淡値）が同様のデータ点は同一高さにあることを示し、図１６（ｂ）に示すように、階段ＳＴ２においても１段目より２段目、２段目より３段目の踏面にあたる領域のデータ点の濃淡が薄くなっている。また、距離データが図示されている略台形領域がロボット装置が撮影可能な範囲（視野範囲）を示す。また、同図において、距離データ点は略４段階の濃淡に分かれているが、ｚ方向が最も小さい領域に対応する最も濃淡が濃い箇所は、床面を示している。また、図１６（ｃ）は、横軸をｙ方向、縦軸をｚ方向、ｘ方向を色の濃淡で示すものである。この図では、ｘ方向の距離が大きくなるほど濃淡が薄くなるように表現されている。図１６（ｄ）は、横軸をｘ方向、縦軸をｚ方向とし、ｙ方向をその距離に応じて濃淡で表現している。

また、ロボット装置２０１が階段ＳＴ２の側面を撮影した場合は、図１７に示すように、ｘ軸が大きい上方側の領域に存在するデータ点が、高さが０と同様の濃淡を示しており、階段ＳＴ２の奥の床面を計測した結果であることを示している。また、図１８に示す斜め方向からの撮像においても、床面及び１〜３段目の踏面を示す４つの領域が、高さの違いに応じて異なる濃淡で示され、はっきり区別できていることを示している。

ここで、このような３次元距離データをステレオカメラによって取得するためには、階段ＳＴ２の表面に模様（テクスチャ）が必要となる。すなわち、２台のカメラによる視差により得ることができるため、模様がないものは視差が算出できず、正確に距離を計測することができない。すなわち、ステレオビジョンシステムにおける距離データの計測精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。

そこで、図１９に示すように、頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源２１を設ける。この光源２１は、模様がない階段ＳＴ３、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射（照射）し、ランダムなパターンＰＴを付与する模様付与手段として作用する。なお、ランダムパターンＰＴを形成して距離画像を取得できるものであれば、ランダムパターンＰＴを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載されるＣＣＤカメラなどにおいては観測可能なパターンを付与することができる。

環境地図作成部４における３次元グリッド更新部４１は、ステレオビジョンシステム２によって取得された３次元情報であるステレオデータＤ１と、この３次元情報に対する平面検出器３による平面検出結果である平面データＤ２を受け取り３次元グリッドを作成する。また、３次元グリッドで表される実空間Ｓ５００における全ての単位グリッド（以下、セルと記す。）ｐ（ｘ，ｙ，ｚ）に対して次の処理を行う。

３次元グリッド更新部４１は、測定点と観測中心（ステレオカメラ中心）とを結ぶ直線上には障害物が存在しないというアルゴリズムに基づいて３次元グリッドを作成及び／又は更新している。図２０は、３次元グリッドが観測結果に応じて更新される様子をｙ軸方向からの側面図として模式的に表したものである。ロボット装置２０１の視野領域（Field of View）にないセル（斜線のセル）は、まだ測定していないセルを表す。ロボット装置２０１によって測定されたセルは黒塗りで表され、測定点にあたるセルとの間のセルは、白塗りで表されている。ハッチング領域は、未確認領域であることを示している。

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

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

このように障害物の存在確率が与えられた３次元グリッドを図１９に示す。本具体例では、３次元グリッドとして表される領域は、例えば、４メートル四方、水平解像度４センチ及び垂直解像度１センチを１セルとした３次元グリッドで表現されている。図２１では、説明の便宜上、図９に示す実空間Ｓ５００と同じ領域が３次元グリッドとして表されているが、実際のロボット装置２０１は、例えば１秒間に３０回というように所定間隔毎に周囲の状態を取得していくため、ロボット装置２０１が移動している場合には、３次元グリッドとして表現される空間Ｓ５００は、時々刻々と変化することになる。取得されたセルは、見えたところ１、見えないところ０．５とする。占有確率は、１秒間に３０回測定するうちに徐々に更新されていく。

環境地図作成装置１における高さ地図検証部４２は、２次元高さ地図（2D Height Map）のセルに対して高さ情報をアナログ情報としてあてはめた２．５次元高さ地図を作成し、３次元グリッドから得られるセルの占有確率データ（障害物存在確率データ）を用いて２．５次元高さ地図を検証する。

２．５次元高さ地図（2D Height Map）によって環境を認識する従来例では、図３６及び図３７を用いて説明したように、前回観測された平面が消えた場合や視野領域外部から視野領域内に障害物が移動する場合などにセルの障害物占有確率が更新されないという問題点が生じていたが、本具体例では、高さ地図検証部４２で２．５次元高さ地図を３次元グリッドを用いて検証することによって、この問題を解決している。３次元グリッドは、幾何学的に測定点と観測中心（ステレオカメラ中心）とを結ぶ直線上には、障害物は存在しないというアルゴリズムに基づいて作成及び／又は更新される。高さ地図検証部４２は、２．５次元高さ地図のセルの高さ情報を３次元グリッドのセルに対応付けしたときの占有確率を３次元グリッドのセルの占有確率を参照して更新する処理を行っている。

以下に、高さ地図検証部４２における検証処理を説明する。図２２は、２．５次元高さ地図を示す。２．５次元高さ地図は、矩形状格子で表される２次元グリッドの各セルに対して、高さ情報ｈの情報と無効フラグ（invalid Flag）とを保持している。図２２では、図９における平面ＰＬ５０１に対応するセルに高さｈ＝３０が、平面ＰＬ５０２に対応するセルに高さｈ＝６０が与えられている。また、このセルがロボット装置２０１には登行不可能な高さ情報をもっている場合には、無効フラグ（inv）が与えられる。

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

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

高さ地図検証部４２は、２．５次元高さ地図内の全セル（ｉ，ｊ）に対して２．５次元高さ地図を参照してグリッドの高さを取得する（ステップＳ１１）。高さ情報ｈに無効フラグが立っていた場合、処理を終了する（ステップＳ１２；ｙｅｓ）。

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

閾値より小さい場合（ステップＳ１４；ｎｏ）、このセルは障害物でないと判断する。このとき、後段に示す高さ地図更新部４３において２．５次元高さ地図のセル（ｉ，ｊ）を３次元グリッドを作成するアルゴリズムに基づいて更新する。閾値より大きい場合（ステップＳ１４；ｙｅｓ）、このセルは障害物であると判断し処理を終了する。すなわち、この場合には、２．５次元高さ地図の高さ情報ｈを更新しない。

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

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

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

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

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

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

図２５は、この一連の処理によって作成された環境地図を表している。上述したように、２．５次元高さ地図を３次元グリッドによって補完することで、図３６及び図３７を用いて説明した不具合が解消できる。環境地図は、２次元グリッドのセルに対して高さ情報ｈ又は障害物（obs）か否かがあてはめられたものになっている。

以下では、上述の環境地図作成方法によってロボット装置２０１が作成した環境地図において、ロボット装置２０１が一度認識した平面が消失した場合、また認識した平面が移動した場合について、２．５次元高さ地図だけによる環境表現結果と、３次元グリッドによって２．５次元高さ地図を補完したときの環境表現結果とを比較する。

１．平面が消失した場合
ロボット装置２０１が視野領域内の箱Ｚをみて認識する様子を図２６に示す。また、図２７は、図２６において認識された箱Ｚがなくなった状態を示している。ロボット装置２０１が図２６の状態から作成した環境地図が図２８に示されている。セルの色分けは、高さ情報ｈが異なることを表している。図２８では、ロボット装置２０１が目前の矩形の箱Ｚに対応する平面を検出していることが示されている。

図２９は、２．５次元高さ地図のみにより作成された環境地図を表している。また、図３０は、２．５次元高さ地図を３次元グリッドによって補完して作成された環境地図を表している。前者の場合、既に箱Ｚが取り除かれ、箱Ｚによる平面がなくなっているのにも関わらず、図３６及び図３７を用いて説明した不具合によって、箱Ｚがあるときには、認識されていたが取り除かれることによってロボット装置２０１からは見えなくなった領域のセルが更新されず、箱Ｚの一部が依然として平面として誤認識されてしまう。一方、本発明の具体例として示す環境地図作成方法によって作成された後者の環境地図は、３次元グリッド作成のアルゴリズムによって検証することで、この残留セルが正しく更新される。

２．平面が移動した場合
この例では最初、ロボット装置２０１の目前には何も置かず、暫くした後にロボット装置２０１の視野領域外から箱Ｚをゆっくり視野領域内に移動させている。このとき、ロボット装置２０１が視野領域内の箱Ｚをみて認識する様子を図３１及び図３２に示す。図３２は、図３１の状態ではなかった箱Ｚが視野領域内に移動される状態を示している。ロボット装置２０１が図３１の状態から作成した環境地図が図３３に示されている。セルの色分けは、高さ情報ｈが異なることを表している。

図３４は、２．５次元高さ地図のみにより作成された環境地図を表している。また、図３５は、２．５次元高さ地図を３次元グリッドによって補完して作成された環境地図を表している。前者の場合、移動されてきた箱Ｚの上面によって遮蔽されたロボット装置２０１からは見えなくなったセルが更新されることなく依然として平面として誤認識されている。一方、本発明の具体例として示す環境地図作成方法によって作成された後者の環境地図は、３次元グリッド作成のアルゴリズムによって検証することで、この残留セルが正しく更新される。

ロボット装置２０１は、上述した環境地図作成方法によって作成された環境地図に基づいて、経路決定部（Path Planner）７によって行動経路を決定することができる。行動経路の決定には、例えば、環境地図に基づいて認識した環境（空間Ｓ５００）内を実際に移動することによって行動可能な領域を確認する手法のほか、従来の２次元グリッドを用いた経路探索手法を適用することができる。

以上説明したように、本発明の具体例として示す環境地図作成方法によれば、平面検出された平面だけが、２．５次元高さ地図として表現される。それ以外のセルは、障害物として表現される。セルには、高さ情報ｈがあてはめられる。また、ロボット装置２０１が登行不可能な高さ情報ｈに対しては、障害物（Obstacle）、階段（Staircase）のように表現してもよい。

本発明の具体例として示す環境地図作成方法によれば、観測ノイズに対して脆弱な２．５次元平面地図を、セル毎の障害物存在確率を統計的に処理することのできる３次元グリッドによって補完することにより、観測ノイズに対して高い耐性（robust）を有する環境地図作成が行える。また、３次元グリッドは、垂直方向の解像度がセルサイズに依存するため、検出精度を高めることが処理速度の低下に繋がっていたが、本発明の具体例として示す環境地図作成方法では、粗めの解像度で作成した３次元グリッドを使用して演算量を抑えつつ高さ情報ｈを２．５次元高さ地図のアナログ値として保持することができるために、少ないデータ量で高精度の高さ表現が可能となる。このように、本発明の具体例として示す環境地図作成方法は、観測ノイズに対する脆弱性を解決し、平面検出、登行可能な階段検出といった高さ方向の細かい解像度を必要とするものに対しては有利であるという２．５次元高さ地図の利点を生かすことができる。

本発明の具体例として示す環境地図作成装置の概略を説明するブロック図である。上記環境地図作成装置が取得したステレオデータに基づいて環境を環境地図として表現するまでの処理を実行するシステムを説明する機能ブロック図である。上記環境地図作成装置における環境地図作成部における環境地図作成処理を実行するシステムを説明する機能ブロック図である。本発明の実施の形態におけるロボット装置の概観を示す斜視図である。上記ロボット装置が具備する関節自由度構成を模式的に示す図である。上記ロボット装置の制御システム構成を示す模式図である。図１に示した環境地図作成装置をロボット装置に適用したとき周囲の環境を環境地図として認識するための構成を説明する概略図である。ロボット装置が画像入力装置２５１において取得したステレオデータに基づいて環境を環境地図として表現し環境地図に基づいて行動経路を決定するまでの処理を実行するシステムを示す機能ブロック図である。上記環境地図作成装置を備えるロボット装置によって認識される実空間を説明する模式図である。上記ロボット装置が外界を撮影している様子を示す模式図である。本実施の形態における平面検出装置を示す機能ブロック図である。本実施の形態における線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出処理を示すフローチャートである。階段を示す模式図であって、（ａ）は階段を正面からみた図、（ｂ）は階段を側面からみた図、（ｃ）は階段を斜めからみた図である。階段のほかの例を示す模式図であって、（ａ）は階段を正面からみた図、（ｂ）は階段を側面からみた図、（ｃ）は階段を斜めからみた図である。（ａ）は図１５に示す階段をステレオビジョンシステムによって前方から撮影した場合の画像を示す模式図、（ｂ）乃至（ｃ）は（ａ）に示す画像から取得した３次元の距離データを示す図である。（ａ）は図１５に示す階段をステレオビジョンシステムによって側方から撮影した場合の画像を示す模式図、（ｂ）乃至（ｃ）は（ａ）に示す画像から取得した３次元の距離データを示す図である。（ａ）は図１５に示す階段をステレオビジョンシステムによって斜め前方から撮影した場合の画像を示す模式図、（ｂ）乃至（ｃ）は（ａ）に示す画像から取得した３次元の距離データを示す図である。テクスチャを付与する手段を有しているロボット装置を説明するための図である。３次元グリッドが観測結果に応じて更新される様子をｙ軸方向からみた模式図の側面図である。図９に示す実空間と同じ領域を表す３次元グリッドを説明する模式図である。２．５次元高さ地図を示す模式図である。高さ地図検証部による２．５次元高さ地図の検証処理を説明するフローチャートである。高さ地図更新部による高さ地図更新処理を説明するフローチャートである。上記環境地図作成部により作成された環境地図を説明する図である。ロボット装置が視野領域内の箱Ｚをみて認識する様子を示す図である。図２６において認識された箱Ｚがなくなった状態を示す図である。ロボット装置が図２６の状態から作成した環境地図を表す図である。２．５次元高さ地図のみにより作成された環境地図を表す図である。２．５次元高さ地図を３次元グリッドによって補完して作成された環境地図を表す図である。ロボット装置が視野領域内の箱Ｚをみて認識する様子を示す図である。図３１の状態ではなかった箱Ｚが視野領域内に移動された状態を示す図である。ロボット装置が図３１の状態から作成した環境地図を表す図である。２．５次元高さ地図のみにより作成された環境地図を表す図である。２．５次元高さ地図を３次元グリッドによって補完して作成された環境地図を表す図である。従来の２．５次元高さ地図を用いた環境認識の問題点を説明する図である。従来の２．５次元高さ地図を用いた環境認識の問題点を説明する図である。

符号の説明

１環境地図作成装置、２ステレオビジョンシステム、３平面検出器、４環境地図作成部、３１線分抽出部、３２領域拡張部、４１３次元グリッド更新部、４２高さ地図検証部、４３高さ地図更新部

Claims

外部状態に基づいて３次元地図を作成する環境地図作成方法であって、
外部状態検出手段が、外部状態を検出する外部状態検出工程と、
３次元地図作成手段が、上記外部状態検出工程により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成工程と、
対基準面高さ地図作成手段が、上記外部状態検出工程により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成工程と、
高さ地図情報変更手段が、上記３次元地図の情報に基づいて上記対基準面高さ地図の情報を変更する高さ地図情報変更工程と
を有する環境地図作成方法。
上記高さ地図情報変更工程では、上記高さ地図情報変更手段が、上記３次元地図作成工程により更新された３次元グリッドの占有状態に基づいて、上記対基準面高さ地図で表される高さ位置に対応する３次元グリッドの占有状態を判別し、判別結果に応じて上記対基準面高さ地図の情報を変更する請求項１記載の環境地図作成方法。
上記外部状態検出工程は、上記外部状態検出手段が、観測点から測定対象点までの３次元の距離データを計測する距離計測工程を有し、
上記３次元地図作成工程では、上記３次元地図作成手段が、上記距離計測工程により計測された３次元距離データに基づいて上記３次元地図の３次元グリッドの占有状態を更新する請求項１記載の環境地図作成方法。
上記外部状態検出工程は、上記外部状態検出手段が、観測点から測定対象点までの３次元の距離データを計測する距離計測工程を有し、
上記対基準面高さ地図作成工程は、平面検出手段が、上記距離計測工程により計測された３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出工程を有する請求項１記載の環境地図作成方法。
上記３次元地図作成工程では、上記３次元地図作成手段が、観測点と測定対象点とを結ぶ直線上に含まれる３次元グリッドの占有確率を低下させ、測定対象点にあたるグリッド単位の占有確率を増加させる請求項３記載の環境地図作成方法。
上記高さ地図情報変更工程では、上記高さ地図情報変更手段が、上記対基準面高さ地図における高さ情報を取得し、該高さ情報を上記３次元地図の高さ座標に対応させ、対応した３次元グリッドの１グリッド単位の占有確率が３次元グリッドにおいて障害物と認識するか否かを決めている閾値より大きいか否かを比較し、閾値より小さい場合、この１グリッド単位は障害物でないと判断し、閾値より大きい場合、この１グリッド単位は障害物であると判断し、対応する対基準面高さ地図の高さ情報を更新する請求項１記載の環境地図作成方法。
上記高さ地図情報変更工程では、上記高さ地図情報変更手段が、上記３次元地図及び上記対基準面高さ地図のうち上記外部状態検出工程において検出される実空間に相当する地図領域に対してのみ高さ地図情報を変更する請求項１記載の環境地図作成方法。
上記平面検出工程では、線分抽出手段が、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出工程と、
平面領域拡張手段が、上記線分抽出工程によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張工程とを有し、
上記線分抽出工程では、上記線分抽出手段が、距離データ点の分布に応じて適応的に線分を抽出することにより平面を検出する請求項４記載の環境地図作成方法。
上記３次元地図は、移動体が移動可能な領域を３次元的に判別するためのものである請求項１記載の環境地図作成方法。
外部状態に基づいて３次元地図を作成する環境地図作成装置であって、
外部状態を検出する外部状態検出手段と、
上記外部状態検出手段により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成手段と、
上記外部状態検出手段により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成手段と、
上記３次元地図の情報に基づいて上記対基準面高さ地図の情報を変更する高さ地図情報変更手段と
を有する環境地図作成装置。
上記高さ地図情報変更手段は、上記３次元地図作成手段により更新された３次元グリッドの占有状態に基づいて、上記対基準面高さ地図で表される高さ位置に対応する３次元グリッドの占有状態を判別し、判別結果に応じて上記対基準面高さ地図の情報を変更する請求項１０記載の環境地図作成装置。
上記外部状態検出手段は、観測点から測定対象点までの３次元の距離データを計測する距離計測手段を有し、
上記３次元地図作成手段は、上記距離計測手段により計測された３次元距離データに基づいて上記３次元地図の３次元グリッドの占有状態を更新する請求項１０記載の環境地図作成装置。
上記外部状態検出手段は、観測点から測定対象点までの３次元の距離データを計測する距離計測手段を有し、
上記対基準面高さ地図作成手段は、上記距離計測手段により計測された３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段を有する請求項１０記載の環境地図作成装置。
上記３次元地図は、移動体が移動可能な領域を３次元的に判別するためのものである請求項１０記載の環境地図作成装置。
移動手段により移動可能なロボット装置において、
外部状態を検出する外部状態検出手段と、
上記外部状態検出手段により検出された外部状態に基づいて３次元グリッドの占有状態を表す３次元地図を作成する３次元地図作成手段と、
上記外部状態検出手段により検出された外部状態に基づいて２次元の基準面からの高さを表す対基準面高さ地図を作成する対基準面高さ地図作成手段と、
上記３次元地図の情報に基づいて上記対基準面高さ地図の情報を変更する高さ地図情報変更手段と、
上記高さ地図情報変更手段によって変更された地図情報を環境地図として自律的に行動経路を決定し移動動作を制御する移動制御手段と
を備える移動型ロボット装置。