WO2005087452A1

WO2005087452A1 - ロボット装置、及びその動作制御方法、並びに移動装置

Info

Publication number: WO2005087452A1
Application number: PCT/JP2005/004838
Authority: WO
Inventors: Steffen Gutmann; Masaki Fukuchi
Original assignee: Sony Corp
Current assignee: Sony Corp
Priority date: 2004-03-17
Filing date: 2005-03-17
Publication date: 2005-09-22
Anticipated expiration: 2006-09-17
Also published as: JPWO2005087452A1; JP4618247B2; WO2005087452A9

Abstract

　ロボット装置は、ステレオビジョンシステム（１）によって外界を観測し、両眼の視差によって算出される３次元距離情報であるステレオデータ（Ｄ１）を距離画像として出力する。平面検出器（２）は、この距離画像から平面を検出することで、環境内に存在する複数の平面を認識する。階段認識器（３）は、平面データ（Ｄ２）からロボット装置が昇降可能な平面を抽出し、その平面から階段を認識し階段データ（Ｄ４）を出力する。階段昇降制御器（４）は、この階段データ（Ｄ４）を用いて階段の昇降動作を実現する動作制御指令（Ｄ５）を出力する。これによって、移動体自身が階段に係る情報を取得して自律的に階段昇降動作を可能とする。

Description

ロボット装置、及びその動作制御方法、並びに移動装置

技術分野

[0001] 本発明は、例えば脚部などの移動手段を有し、複数段からなる階段の昇降動作等を可能とするロボット装置、移動装置、及びその階段昇降方法に関する。

本出願は、日本国において 2004年 3月 17日に出願された日本特許出願番号 200 4 077214を基礎として優先権を主張するものであり、これらの出願は参照することにより、本出願に援用される。

背景技術

[0002] 従来、車輪等の移動手段を有する移動装置を含むロボット装置に対し、段差のある環境や、階段の昇降動作をさせることを可能とする技術が複数開示されて、る。例えば、対象物体の形状特徴点を認識するため対象物体のノードデータを有した、図 1に示す既知の階段情報を用いて昇降動作を行う方法 (特許第 3176701号公報)や、図 2に示すように、多数の接触センサ 602が保護膜 603で被覆されつつマトリタス状に足平 601の裏面の全面的に貼り付けられた足平を利用して、階段昇降動作を行う方法がある（特許第 3278467号公報)。更に、図 3に示すように、足底の脇に赤外センサを装備し、階段にランドマークテープを付与することで、階段昇降動作を行う方法がある（特許第 3330710号公報)。これは、図 3Aに示すように、 2足歩行可能なロボット装置の足平 622RZLの左右の脇部分に複数の光センサ検出部 682を設け、黒色ペイントなどの光をよく吸収する塗料で描、た直線力もなる所定幅の面領域であるランドマーカ 680を利用することで、対になっているセンサ出力を比較することでランドマーカ 680に対する相対的な方角を検出するものである。このような足平 6 22RZLとランドマーカ 680とを使用することで、図 3Bに示すように、階段 690の位置を認識することが可能となる。

し力しながら、特許第 3176701号公報に記載の技術においては、既知の階段情報に基づいているため、未知の環境においては適応することができなぐ例えば自律型のロボット装置などに適用することが困難である。また、特許第 3278467号公報及び特許第 3330710号公報に記載の技術においては、足裏もしくは足底の脇に設けられた複数のセンサを利用して階段昇降動作を行うため、着床するまで階段の情報が取得できない。したがって、遠くからの観測が不可能である。このため、ある程度目の前に階段があることが予測されるときにし力利用することができない。

発明の開示

発明が解決しょうとする課題

本発明は、このような従来の実情に鑑みて提案されたものであり、移動体自身が階段に係る情報を取得して自律的に階段昇降動作を可能とするロボット装置及び移動装置、並びにロボット装置の動作制御方法を提供することを目的とする。

上述した目的を達成するために、本発明に係るロボット装置は、移動手段により移動可能なロボット装置において、 3次元の距離データから環境内に含まれる 1又は複数の平面を検出し、平面情報として出力する平面検出手段と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有することを特徴とする。

本発明においては、移動手段として例えば脚部などを備えて移動可能なロボット装置において、階段の踏面の例えば大きさや位置などに関する踏面情報から、その足底がその踏面に載せることができる大きさか否かを判断したり、階段の段差を示す蹴り上げの情報力その高さの踏面への移動が可能力否かを判断し、移動可能であると判断した場合には自律的に位置決めすることで階段を登ったり降りたりすることが可能となる。

また、上記 3次元の距離データを取得する距離計測手段を有することができ、階段を検出した際には自律的に昇降動作が可能となる。

また、上記階段認識手段は、与えられた平面情報力移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出手段と、上記階段検出手段から出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合手段とを有することができ、例えば視野が狭いロボット装置であったり、 1度の検出処理ではうまく階段認識できないような場合であっても、時間的に統計的に統合した統合済階段情報とすることで、正確かつ高域な認識結果を得ることができる。

更に、上記階段検出手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、上記階段統合手段は、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である 2以上の踏面力なる踏面群を検出した場合、当該踏面群を何れをも含む一の踏面となるよう統合することができ、統合の際には統合すべきとして選択された踏面を全て含むように統合することで、広、範囲にわたって認識結果を得ることができる。

更にまた、上記階段認識手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し上記踏面情報とすることができ、踏面情報は、少なくとも移動方向に対して該踏面の手前側の境界を示すフロントエッジ及び奥側の境界を示すバックェッジの情報を含むものとすることができ、踏面のフロントエッジ及びバックエッジを認識するため、例えばスパイラル形状の階段などであっても正確に踏面を認識して階段昇降動作を可能にする。

また、上記フロントエッジ及びバックエッジに挟まれた領域である安全領域の左右両側に隣接した領域であって移動可能である確率が高いと推定されるマージン領域を示す右側マージン情報及び左側マージン情報、上記平面情報に基づき踏面と推定された領域の重心を示す参照点情報、踏面となる平面を構成する点群の 3次元座標情報などを有することができる。これらの踏面情報を使用することで、階段昇降動作をより正確に制御することができる。

更に、上記階段認識手段は、上記平面情報に基づき平面の境界を抽出して多角形を算出し、該多角形に基づき上記踏面情報を算出することができ、例えば、視野が狭い場合、 3次元距離データの信頼性が高いような場合には、上記多角形は、上記平面情報に基づき抽出された平面の境界に外接する凸多角形領域とすることができ、実際に検出されている平面を含んだ領域とすることができる。一方、ノイズが多い距離データなどの場合には、上記多角形は、上記平面情報に基づき抽出された平面の境界に内接する凸多角形領域とすることができ、実際に検出されている平面に内包される領域とすることで、ノイズ部分をカットして正確に踏面を検出することができる。

更にまた、現在移動中の移動面におけるバックエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御することができ、例えば蹴り上げが小さい階段など、フロントエッジとバックエッジが重なるような場合には、ノックエッジを目標に移動して昇降動作することができる。同様に、フロントエッジを目標に移動して昇降動作を行ってもよい。

また、上記階段昇降制御手段は、現在移動中の移動面におけるバックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御することができ、例えば床面を移動していて階段を検出した場合、床面のノックエッジが階段初段のフロントエッジに重ならない場合があり、そのような場合は階段の初段、すなわち昇降動作の対象となる次段の踏面のフロントエッジを目標に移動して昇降動作をすることができる。

また、上記階段昇降制御手段は、次に移動対象となる踏面を検出し、当該移動対象となる踏面に対畤した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御することができ、新たな踏面に移動する毎に、踏面に対してサーチ'ァライン'アプローチ動作を実行することで昇降動作を可能にする。

更に、上記階段昇降制御手段は、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報力当該移動対象となる次段の踏面を検索することができ、予め数段上又は下の階段情報を取得しておくことで、ロボット装置が自身の直近の情報が得られないような構成で視野が狭い場合であっても昇降動作を可能とする。

また、上記階段昇降制御手段は、現在の移動面におけるバックエッジに対畤した所定位置に移動した後、次の移動対象となる踏面を検出し、当該踏面におけるフロントエッジに対畤した所定位置に移動し、当該踏面に移動する昇降動作を実行するよう制御することができ、フロントエッジ及びバックエッジを使用することで、両エッジが平行して、な、螺旋状の階段であっても昇降動作を可能とする。

更に、上記昇降制御手段は、踏面に対する上記移動手段の位置を規定したパラメータを使用して昇降動作を制御することができ、このパラメータは、例えば上記脚部の足上げ高さ又は足下げ高さに基づき決定されることができる。そして、階段を登る動作と降りる動作とで上記パラメータの数値を変更するパラメータ切り替え手段を有することができ、階段を登る動作であっても、降りる動作であってもノメータ変更するのみで同様に制御することができる。

また、上記平面検出手段は、 3次元空間で同一平面上にあると推定される距離デ一タ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、

上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することができ、線分抽出手段は、 3次元の距離データが同一平面上にある場合同一直線上に並ぶことを利用して線分を抽出するが、この際、ノイズなどの影響により距離データ点の分布に違いが生じるため、この距離データの分布に応じて適応的に線分を抽出する（Adaptive Line Fitting)ことにより、ノイズに対してロバストに、精確な線分抽出を可能とし、抽出された多数の線分力線分拡張法により平面を求めるため、ノイズの影響などにより、本来複数平面が存在するのに 1つの平面としたり、 1つの平面しか存在しないのに複数平面としたりすることなく精確に平面抽出することができる。更に、上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にある力否かを再度推定することができ、距離データ点の 3次元空間における距離に基づき一旦距離データ点群を抽出しておき、データ点の分布に基づき再度同一平面上にある力否かを推定することにより精確に線分抽出することができる。

更にまた、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第 1の線分を抽出し、該距離データ点群のうち該第 1の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第 2の線分を抽出し、該第 2の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することができ、例えば抽出したデータ点群の端点を結ぶ線分を第 1の線分とし、上記距離が大きい点が存在する場合には、例えば最小二乗法により第 2の線分を生成し、この第 2の線分において一方側に連続して複数のデータ点が存在する場合には、データ点群は例えば線分に対してジグザグな形などをとつていることが想定でき、したがって抽出したデータ点群には偏りがあると判断して、上記着目点などにてデータ点群を分割することができる。

また、上記平面領域拡張手段は、同一の平面に属すると推定される 1以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新すると共に該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力することができ、同一平面に属するとされる線分により平面領域拡張処理及び平面更新処理を行うことができる。

更に、上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除!ヽた距離データ点群力も再度平面を算出する平面再算出手段を更に有することができ、更新済平面はそれに属する全線分の平均した平面として得られているため、これ力大きく外れた距離データ点を除いたデータ点群力再度平面を求めることで、よりノイズなどの影響を低減した検出結果を得ることができる。

更にまた、上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定することができ、例えば平面方程式の 2乗平均誤差などに基づきノイズの影響であるのか、異なる平面なのかを判別して更に正確に平面検出することができる。

本発明に係るロボット装置の動作制御方法は、移動手段により移動可能なロボット装置の動作制御方法において、 3次元の距離データから環境内に含まれる 1又は複数の平面を検出し、平面情報として出力する平面検出工程と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程とを有することを特徴とする。

本発明に係る移動装置は、移動手段により移動可能な移動装置において、 3次元の距離データから環境内に含まれる 1又は複数の平面を検出し、平面情報として出力する平面検出手段と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有することを特徴とする。

本発明によれば、移動手段として例えば脚部などを備えて移動可能なロボット装置及び移動装置において、階段の踏面の例えば大きさや位置などに関する踏面情報から、その足底がその踏面に載せることができる大きさか否かを判断したり、階段の段差を示す蹴り上げの情報力その高さの踏面への移動が可能力否かを判断し、移動可能であると判断した場合には自律的に位置決めすることで階段を登ったり降りたりすることが可能となる。

本発明の更に他の目的、本発明によって得られる利点は、以下において図面を参照して説明される実施に形態から一層明らかにされるであろう。

図面の簡単な説明

[図 1]図 1は、従来の昇降動作を説明する図である。

[図 2]図 2は、従来の昇降動作を説明する図である。

[図 3]図 3A、図 3Bは、従来の昇降動作を説明する図である。

[図 4]図 4は、本発明の実施の形態におけるロボット装置の概観を示す斜視図である

[図 5]図 5は、ロボット装置が具備する関節自由度構成を模式的に示す図である。

[図 6]図 6は、ロボット装置の制御システム構成を示す模式図である。 [図 7]図 7は、ロボット装置力ステレオデータから階段昇降動作を発現するまでの処理を実行するシステムを示す機能ブロック図である。

[図 8]図 8Aは、ロボット装置が外界を撮影している様子を示す模式図、図 8Bは、ロボット装置の足底の大きさを示す図である。

[図 9]図 9は、階段検出を説明する図であって、図 9Aは、階段を正面から見た図、図 9Bは、階段を側面から見た図、図 9Cは、階段を斜め力も見た図である。

[図 10]図 10は、階段検出の他の例を示す説明する図であって、図 10Aは、階段を正面から見た図、図 10Bは、階段を側面から見た図、図 10Cは、階段を斜めから見た図である。

[図 11]図 11は、図 9の階段を検出した結果の一例を示す図であって、図 11Aは、図 9の階段を撮影した場合の画像を示す模式図、図 11B乃至図 11Dは、図 11Aに示す画像から取得した 3次元の距離データを示す図である。

[図 12]図 12は、図 10の階段を検出した結果の一例を示す図であって、図 12Aは、図 10の階段を撮影した場合の画像を示す模式図、図 12B乃至図 12Dは、図 12Aに示す画像から取得した 3次元の距離データを示す図である。

[図 13]図 13Aは、階段を撮影した画像を示す模式図、図 13Bは、図 13Aから取得した 3次元距離データ力 4つの平面領域 A、 B、 C、 Dを検出した結果を示す図である

。階段を検出した結果の一例を示す図である。

[図 14]図 14は、階段認識器を示す機能ブロック図である。

[図 15]図 15は、階段検出処理の手順を示すフローチャートである。

[図 16]図 16A、図 16Bは、多角形を示す模式図である。

[図 17]図 17は、 Melkmanのアルゴリズムを説明するための模式図である。

[図 18]図 18A、図 18Bは、 Sklanskyのアルゴリズムにより多角形を求める方法を説明するための模式図である。

圆 19]図 19は、非凸多角形形状の階段について発生する問題を説明するための模式図であって、図 19Aは、入力される平面を示す図、図 19Bは、凸包による非凸多角形形状の階段の多角形表現結果を示す図である。

[図 20]図 20は、平滑ィ匕によって入力平面を包含する多角形を求める方法を示す模式図であって、図 20Aは、入力された平面を示す図、図 20Bは、入力平面を示す多角形力も不連続なギャップを除去し平滑ィ匕した多角形を示す図、図 20Cは、図 20B で得られた多角形に対してラインフィッティングにより更に平滑ィ匕した多角形を示す図である。

[図 21]図 21は、ギャップ除去とラインフィットによる平滑ィ匕によって入力平面を包含する多角形を求める処理のプログラム例を示す図である。

[図 22]図 22A、図 22Bは、階段パラメータの算出方法を説明するための模式図である。

[図 23]図 23は、最終的に認識される踏面及び階段パラメータを説明するための模式図である。

[図 24]図 24A、図 24Bは、階段を示す模式図である。

[図 25]図 25は、階段統合処理の方法を示すフローチャートである。

[図 26]図 26は、オーバーラップしている階段データを統合する処理を説明するための模式図である。

[図 27]図 27は、ァライン動作を説明するための図である。

[図 28]図 28は、アプローチ動作を説明するための模式図である。

[図 29]図 29は、階段昇降動作の手順を示すフローチャートである。

[図 30]図 30は、サーチ ·ァライン ·アプローチ処理方法を示すフローチャートである。

[図 31]図 31は、昇降動作処理の方法を示すフローチャートである。

[図 32]図 32は、ロボット装置が認識しているか又は認識する予定の階段面を示す模式図である。

[図 33]図 33は、昇降動作処理の方法を示すフローチャートである。

[図 34]図 34は、昇降動作処理の方法を示すフローチャートである。

[図 35]図 35Aは、ロボット装置により認識されている踏面と足底の関係を説明するための図であり、図 35Bは、各部の寸法を示す図である。

[図 36]図 36は、ロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。

[図 37]図 37は、ロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。

[図 38]図 38は、単一の段部とロボット装置の足底の関係を示す図である。

[図 39]図 39は、単一の凹部とロボット装置の足底の関係を示す図である。

[図 40]図 40は、本変形例における平面検出装置を示す機能ブロック図である。

[図 41]図 41は、テクスチャを付与する手段を有しているロボット装置を説明するための図である。

[図 42]図 42は、本変形例における線分拡張法による平面検出方法を説明する図である。

[図 43]図 43は、線分拡張法による平面検出処理を示すフローチャートである。

[図 44]図 44は、本変形例における線分抽出部における処理の詳細を示すフローチヤートである。

[図 45]図 45は、距離データ点の分布の様子を示す図であって、図 45Aは、データの分布が線分に対してジグザグ形である場合、図 45Bは、ノイズなどにより線分近傍に一様に分布して、る場合を示す模式図である。

[図 46]図 46は、本変形例における Zig-Zag-Shape判別方法を示すフローチャートである。

[図 47]図 47は、上記 Zig-Zag-Shape判別処理のプログラム例を示す図である。

[図 48]図 48は、 Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。

[図 49]図 49は、本変形例における領域拡張処理を説明するための模式図である。

[図 50]図 50は、本変形例における領域拡張部における領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。

[図 51]図 51は、端点と直線との距離が等しくても平面方程式の 2乗平均誤差 rmsが異なる例を示す図であって、図 51Aは、ノイズなどの影響により線分が平面力もずれている場合、図 51Bは、線分が属する他の平面が存在する場合を示す模式図である

[図 52]図 52は、領域種の選択処理を示す図である。

[図 53]図 53は、領域拡張処理を示す図である。

[図 54]図 54Aは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図 5 4 Bは、縦軸を X、横軸を y、各データ点の濃淡で z軸を表現して 3次元距離データ及び、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す図、図 5 4 Cは、図 5 4 Bに示す直線群から領域拡張処理により得られた平面領域を示す図である。

【図 5 5】図 5 5は、床面に段差を一段置いたときの本変形例における平面検出方法と従来の平面検出方法との結果の違いを説明するための図であって、図 5 5 Aは、観察された画像を示す模式図、図 5 5 Bは、実験条件を示す図、図 5 5 Cは、本変形例における平面検出方法により平面検出された結果を示す図、図 5 5 Dは、従来の平面検出方法により平面検出された結果を示す図である。

【図 5 6】図 5 6 Aは、床面を撮影した画像を示す模式図、図 5 6 B及び図 5 6 Cは、図 5 6 Aに示す床面を撮影して取得した 3次元距離デ一夕から水平方向及び垂直方向の距離デ一夕点列から、それぞれ本変形例の線分検出により検出した線分及び従来の線分検出により検出した線分を示す図である。

発明を実施するための最良の形態

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、周囲の環境に存在する階段などの段差を認識する段差認識装置を搭載した自律的に動作可能な口ポット装置に適用したものである。

本実施の形態におけるロポット装置は、ステレオビジョンなどにより得られた距離情報

(距離データ）から抽出した複数平面から階段を認識し、この階段認識結果を利用して階段昇降動作を可能とするものである。

( 1 ) 口ポット装置

ここでは、まず、このような口ポット装置の一例として 2足歩行タイプの口ポット装置を例にとって説明する。この口ポット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用口ポットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターティンメントロポット装置である。なお、ここでは、 2足歩行型のロボット装置を例にとって説明するが、階段認識装置は、 2足歩行の口ポット装置に限らず、脚式移動型の口ポット装置に搭載すればロボット装置に階段昇降動作を実行させることができる。

差替え用紙（規則 26) 図 4は、本実施の形態におけるロボット装置の概観を示す斜視図である。図 4に示すように、ロボット装置 201は、体幹部ユニット 202の所定の位置に頭部ユニット 203 が連結されると共に、左右 2つの腕部ユニット 204RZLと、左右 2つの脚部ユニット 2 05RZLが連結されて構成されている（ただし、 R及び Lの各々は、右及び左の各々を示す接尾辞である。以下において同じ。 ) o

このロボット装置 201が具備する関節自由度構成を図 5に模式的に示す。頭部ュ- ット 203を支持する首関節は、首関節ョー軸 101と、首関節ピッチ軸 102と、首関節口一ノレ軸 103と!ヽぅ 3自由度を有して！/ヽる。

また、上肢を構成する各々の腕部ユニット 204RZLは、肩関節ピッチ軸 107と、肩関節ローノレ軸 108と、上腕ョー軸 109と、月寸関節ピッチ軸 110と、前腕ョー軸 111と、手首関節ピッチ軸 112と、手首関節ロール輪 113と、手部 114とで構成される。手部 114は、実際には、複数本の指を含む多関節 *多自由度構造体である。ただし、手部 114の動作は、ロボット装置 201の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は 7 自由度を有するとする。

また、体幹部ユニット 202は、体幹ピッチ軸 104と、体幹ロール軸 105と、体幹ョー軸 106という 3自由度を有する。

また、下肢を構成する各々の脚部ユニット 205RZLは、股関節ョー軸 115と、股関節ピッチ軸 116と、股関節ロール軸 117と、膝関節ピッチ軸 118と、足首関節ピッチ軸 119と、足首関節ロール軸 120と、足底 121とで構成される。本明細書中では、股関節ピッチ軸 116と股関節ロール軸 117の交点は、ロボット装置 201の股関節位置を定義する。人体の足底 121は、実際には多関節，多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置 201の足底は、ゼロ自由度とする。したがって、各脚部は、 6自由度で構成される。

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

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

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

思考制御モジュール 200は、情緒判断や感情表現に関する演算処理を実行する C PU (Central Processing Unit) 211や、 RAM (Random Access Memory) 212、 ROM (Read Only Memory) 213及び外部記憶装置（ノヽード ·ディスク ·ドライブ等） 214等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。

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

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

一方の運動制御モジュール 300は、ロボット装置 201の全身協調運動を制御する CPU311や、 RAM312、 ROM313及び外部記憶装置（ノヽード'ディスク'ドライブ等 ) 314等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置 314には、例えば、オフラインで算出された歩行パターンや目標とする ZMP軌道、その他の行動計画を蓄積することができる。

この運動制御モジュール 300には、図 5に示したロボット装置 201の全身に分散するそれぞれの関節自由度を実現するァクチユエータ 350、対象物との距離を測定する距離計測センサ（図示せず)、体幹部ユニット 202の姿勢や傾斜を計測する姿勢センサ 351、左右の足底の離床又は着床を検出する接地確認センサ 352, 353、足底 121の足底 121に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置 354等の各種の装置力バス'インターフェース（IZF) 310経由で接続されている。ここで、姿勢センサ 351は、例えば加速度センサとジャイロ 'センサの組み合わせによって構成され、接地確認センサ 352, 353は、近接センサ又はマイクロ 'スィッチ等で構成される。

思考制御モジュール 200と運動制御モジュール 300は、共通のプラットフォーム上で構築され、両者間はバス.インターフェース 210, 310を介して相互接続されている運動制御モジュール 300では、思考制御モジュール 200から指示された行動を体現すベぐ各ァクチユエータ 350による全身協調運動を制御する。すなわち、 CPU3 11は、思考制御モジュール 200から指示された行動に応じた動作パターンを外部記憶装置 314から取り出し、又は、内部的に動作パターンを生成する。そして、 CPU3 11は、指定された動作パターンに従って、足部運動、 ZMP軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各ァクチユエータ 350に転送する。

また、 CPU311は、姿勢センサ 351の出力信号によりロボット装置 201の体幹部ュニット 202の姿勢や傾きを検出するとともに、各接地確認センサ 352, 353の出力信号により各脚部ユニット 205RZLが遊脚又は立脚の何れの状態であるかを検出することによって、ロボット装置 201の全身協調運動を適応的に制御することができる。更に、 CPU311は、 ZMP位置が常に ZMP安定領域の中心に向力うように、ロボット装置 201の姿勢や動作を制御する。

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

(2)ロボットの謝乍制御去

上述のロボット装置においては、頭部ユニット 203にステレオビジョンシステムを搭載し、外界の 3次元距離情報を取得することができる。次に、このようなロボット装置などに好適に搭載されるものであって、ロボット装置力ステレオビジョンシステムにより周囲の環境力獲得した 3次元距離データを使用して平面を検出し、この平面検出結果に基づき階段を認識し、この階段認識結果を使用して階段昇降動作を行う一連の処理について説明する。

図 7は、ロボット装置力ステレオデータから階段昇降動作を発現するまでの処理を実行するシステムを示す機能ブロック図である。図 7に示すように、ロボット装置は、 3 次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム (Stereo Vision System) 1と、ステレオビジョンシステム 1からステレオデータ D1が入力され、このステレオデータ D1から環境内の平面を検出する平面検出器 (Plane Segmentation/Extractor) 2と、平面検出器 2から出力される平面データ D2から階段を認識する階段認識器 (Stair Recognition) 3と、階段認識部 2により認識された認識結果である階段データ D4を使用して階段昇降動作をするための動作制御指令 D5 を出力する階段昇降制御器 (Stair Climber) 4とを備える。

そして、ロボット装置は、先ず、ステレオビジョンによって外界を観測し、両眼の視差によって算出される 3次元距離情報であるステレオデータ D1を画像として出力する。すなわち、人間の両眼に相当する左右 2つのカメラ力もの画像入力を各画素近傍毎に比較し、その視差カゝら対象までの距離を推定し、 3次元距離情報を画像として出力 (距離画像)する。この距離画像カゝら平面検出器 2によって平面を検出することで、環境内に存在する複数の平面を認識することができる。更に、階段認識器 3によって、これら平面力ロボット装置が昇降可能な平面を抽出し、その平面から階段を認識し階段データ D4を出力する。そして階段昇降制御器 4が階段データ D4を用いて階段の昇降動作を実現する動作制御指令 D5を出力する。

図 8Aは、ロボット装置 201が外界を撮影している様子を示す模式図である。床面を X— y平面とし、高さ方向を z方向としたとき、図 8Aに示すように、画像入力部 (ステレォカメラ）を頭部ユニット 203に有するロボット装置 201の視野範囲は、ロボット装置 2 01の前方の所定範囲となる。

ロボット装置 201は、上述した CPU211において、画像入力装置 251からのカラー画像及び視差画像と、各ァクチユエータ 350の全ての関節角度等のセンサデータとなどが入力されて各種の処理を実行するソフトウェア構成を実現する。

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

ステレオビジョンシステム 1では、カラー画像及びステレオカメラによる視差画像などの画像データが撮像された時間において、センサデータ力も割り出した関節角を使用してロボット装置 201が中心に固定されたロボット中心座標系を頭部ユニット 203 に設けられた画像入力装置 251の座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列とこれに対応する 3次元距離データからなる距離画像を出力する。

本実施の形態におけるロボット装置は、自身の視野内に含まれる階段を認識することができ、その認識結果 (以下、階段データという。）を使用して階段昇降動作を可能とする。したがって、階段昇降動作のためには、ロボット装置は、階段の大きさが自身の足底の大きさより小さいか、階段の高さが登ることができる又は降りることができる高さか否かなどの、階段の大きさについて様々な判断を行う必要がある。ここで、本実施の形態においては、ロボット装置の足底の大きさを図 8Bとした場合について説明する。すなわち、図 8Bに示すように、ロボット装置 201の前進方向を X 軸方向、床面に平行で X方向と直交する方法^ y方向としたとし、ロボット装置 201が直立した際の両足の y方向の幅を feet base width足底の大きさであて足首（脚部と足底の接続部）力ゝら前側の部分を足底前幅 foot_fr₀nt__Si_Ze、足首から後ろ側の部分を足底の足底後ろ幅 foot_back_sizeとするものとする。

ロボット装置 201が環境内から検出する階段としては、例えば図 9、図 10に示すようなものがある。図 9A、図 10Aは、階段を正面から見た図、図 9B、図 10Bは、階段を側面から見た図、図 9C、図 IOCは、階段を斜めから見た図である。

ここで、人間、ロボット装置などが階段を昇降するために使用する面 (足又は可動脚部を載せる面)を踏面と、、一の踏面力その次の踏面までの高さ（階段 1段の階段の高さ）を蹴り上げという。また、以下では、階段は、地面に近い方から登るに従つて 1段目、 2段目とカウントすることとする。

図 9に示す階段 ST1は、段数が 3段の階段であり、蹴り上げ 4cm、 1, 2段面の踏面の大きさは幅 30cm、奥行き 10cm、最上段である 3段目の踏面のみ、幅 30cm、奥行き 21cmとなっている。また、図 10に示す階段 ST2も、段数が 3段の階段であり、蹴り上げ 3cm、 1, 2段面の踏面の大きさは幅 33cm、奥行き 12cm、最上段である 3段目の踏面のみ、幅 33cm、奥行き 32cmとなっている。ロボット装置がこれらの階段を認識した結果は後述する。

平面検出器 2は、ステレオビジョン等の距離計測器力も出力される距離情報 (ステレォデータ D1)力環境内に存在する複数の平面を検出し、平面データ D2を出力する。平面の検出方法としては後述する線分拡張法の他、ハフ変換を利用した公知の平面検出技術を適用することができる。ただし、ノイズを含む距離データ力階段のように複数平面を検出するには、後述する線分拡張法などによる平面検出を行うと正確に平面を検出することができる。

図 11、図 12は、階段を検出した結果の一例を示す図である。図 11及び図 12は、後述する平面検出方法により、それぞれ図 9及び図 10に示す階段を撮影した画像から 3次元距離データを取得して平面検出した例である。すなわち、図 11Aは、図 9 の階段を撮影した場合の画像を示す模式図、図 11B乃至図 11Dは、図 11Aに示す画像から取得した 3次元の距離データを示す図である。また、図 12Aは、図 10の階段を撮影した場合の画像を示す模式図、図 12B乃至図 12Dは、図 12Aに示す画像から取得した 3次元の距離データを示す図である。図 11及び図 12に示すように、何れの場合も全ての踏面を平面として検出できている。図 11Bは、下から 1段目、 2段目、 3段目の踏面が平面検出されている例を示す。また、図 12Bは、床面の一部も他の平面として検出成功して、ることを示す。

すなわち、図 13Aに示すように、例えば階段 ST2を撮影した距離画像力も平面検出すると、図 13Bに示すように、領域 A— Dがそれぞれ、床面、 1段目、 2段目、 3段目の踏面を示す平面として検出される。各領域 A— Dに含まれる同一領域に示す点群は、それぞれ同一平面を構成すると推定された距離データ点群を示している。階段認識器 3には、こうして平面検出器 2が検出した平面データ D2が入力され、階段の形状、すなわち踏面の大きさ、階段の高さ (蹴り上げの大きさ)などを認識する。ここで、本実施の形態における階段認識器 3は、詳細は後述する力ロボット装置 20 1が認識した踏面に含まれる領域 (多角形)の手前側 (ロボット装置に距離的に近い側）の境界（Front Edge) (以下、フロントエッジ FEという。）と、踏面の奥側（ロボット装置に距離的に遠い側）の境界 (Back Edge) (以下、ノックエッジ BEという。）とを階段データとして認識する。そして、階段昇降制御器 4が階段データを利用して階段昇降動作を制御する。

次に、ロボット装置の階段昇降制御方法について具体的に説明する。なお、以下では、第 1に、ロボット装置の階段認識方法、第 2に認識した階段を利用して行う階段昇降動作、最後に平面検出方法の具体例として線分拡張法による平面検出方法の順にて説明する。

図 14は、図 7に示す階段認識器を示す機能ブロック図である。図 14に示すように、階段認識器 3は、平面検出器 2から出力された平面データ D2から階段を検出する階段検出器 (Stair Extraction) 5と、この階段検出器 5が検出した階段データ D3の時系列データ、すなわち異なる時間に検出された複数の階段データ D3を統合することで更に正確に階段を認識する処理を行う階段統合器 (Stair Merging) 6とを有し、階段統合器にて統合した階段データ D4が階段認識器 3の出力となる。

階段検出器 5は、平面検出器 2から入力される平面データ D2から階段を検出するが、

平面検出器 2から入力される平面データ D2は、 1つの平面につき以下に示す複数の情報を有し、ステレオビジョンシステム 1によって取り込まれた画像力も検出された複数の平面毎の平面データが入力される。

すなわち、平面データ D2は、平面毎に

1— 1：平を構成する、の数 (number of supporting point)

1 2 :平面の中心となる点

1-3：平面パラメータ (法線ベクトル、原点からの距離）

1- 4：平面を構成する多角形の境界

から構成される情報を有する。

この平面データに基づき、ロボット装置は、自身が接地している床面や踏面などの接地面と略水平な平面を選択し、下記の情報 (以下、階段パラメータという。）を算出する。すなわち、

2— 1：フロントエッジ FE、バックエッジ BE

2— 2 :階段の高さ

である。

ロボット装置が認識するフロントエッジ FE、ノックエッジ BEとは、上述した如ぐ階段の踏面の境界 (線)を示すものであって、ロボット装置が対畤した場合に、多角形にお Vヽてロボット装置に近、側の境界（手前側の境界）をフロントエッジ FE、ロボット装置から距離が離れている側の境界（奥側の境界）をバックエッジ BEとする。これは、後述するように、例えば平面を構成する点を全て含む最小の多角形を求め、その手前側又は奥側の境界とすることができる。フロントエッジ FE及びバックエッジ BEの情報としては、これらの端点の情報などとすることができる。また、上記多角形から階段の幅 W (width)、階段の長さ (length)などの情報を得ることができる。また、階段の高さ（蹴り上げ）は、与えられた平面データ D2の平面の中心点を利用して、 2の平面の中心点間の高さ差としたり、上記多角形を求めた際の重心点を利用して 2の重心点の高さの差としたりすることができる。なお、蹴り上げは、前段のバックエッジ BEと後段のフロントエッジ FEとの高さの差としたりしてもよい。

また、本実施の形態においては、フロントエッジ FE及びバックエッジ BEに加えて更にフロントエッジ FE及びバックエッジ BEに挟まれた領域 (安全領域）の左右に隣接する領域であって移動可能である確率が高、と推定される領域をマージン (領域)として認識する。これらの求め方については後述する。このマージンを求めることにより、移動可能であると推定する踏面の領域を広く認識することができる。更に、踏面を構成するデータ点群の数や、上述の重心点などを 1つ規定した参照点の情報などの情報（階段パラメータ）のセットを階段データ D3とすることができる。

以上の階段データから下記の条件を満たす平面 (階段)を抽出する。

3—1 :フロントエッジ FE及びバックエッジ BEの長さが所定の閾値以上

3-2：階段の高さ (height)が所定の閾値以下

また、その他、

3-3：階段の幅 W (width)が所定の閾値以上

3-4：階段の長さ L (length)が所定の閾値以上

など同時に満たすものを抽出することが好ましい。

図 15は、階段検出器 5の階段検出処理の手順を示すフローチャートである。図 15 に示すように、先ず、入力された平面データの平面パラメータなどに基づき、入力された平面が例えば接地面と水平であるか否かなど、歩行又は移動可能である平面か否かを判断する (ステップ Sl)。ここで、どのような平面が水平であるか又は移動可能であるかの条件は、ロボット装置の機能に応じて設定すればよい。例えば、入力平面の平面ベクトルを η (η , η , η )とした場合、 | sin n | >min であれば水平である

X y z — 1 z th と判断することができる。ここで、 min は、水平面を判断するための閾値であり、例え

th

ば、使用する距離データ、平面検出など精度を考慮して min =80° などとして、水

th

平面に対して ± 10° までの傾きであれば水平と判断して検出するなどとすることができる。または、例えば、 ± 30° 程度の傾きがあっても歩行または移動可能であれば、それらの角度範囲の平面を抽出するようにすればょ、。ステップ S 1にて水平でなヽと判断した場合 (ステップ Sl :No)、検出に失敗したことを出力して処理を終了し、次の平面データにっ、ての処理を実行する。

次に、平面が水平である場合 (ステップ SI : Yes)、平面の境界 (形状)を認識するための処理を行う。ここでは、例えば Sklanskyのアルゴリズム（J. Sklansky, "Measuring concavity on a rectangular mosaic , IEEE Trans Comput.21,1974,pp.l355- 1364)や Melkmanのアルゴリズム (Melkman A., "On-line Construction of the Convex Hull of a sample Polygon" Information Processing Letters 25, 1987, p.11)などの凸包や、ノィズ除去による平滑ィ匕によって、入力平面を包含する多角形を求める (ステップ S 2)。そして、この多多角形の前後の境界線をフロントエッジ及びバックエッジなどの階段パラメータを求める（ステップ S3)。そして、フロントエッジ及びバックエッジの両境界線から、本実施の形態においては、階段踏面を示す平面における幅 W (width)及び長さ L (length)を求め、これらの値が所定の閾値より大きいかどうか判断する (ステツプ S4)。踏面の幅及び長さが所定の閾値以上でない場合 (ステップ S4 : No)、ロボット装置が移動可能な平面ではな、とし、次の平面データにつ、て再びステップ S 1からの処理を繰り返す。

平面の幅及び長さが所定の閾値以上である場合 (ステップ S4 : No)、移動可能な踏面であると判断し左右のマージン（Left Margin, Right Margin)を計算し（ステップ S 5)、これらの情報を階段データ D3として出力する。

次に、ステップ S2において凸包によって入力平面を包含する多角形を求める方法について説明する。図 16は、凸多角形を示す模式図であって、図 16Aは、入力された一の平面に属すると判断されたサポーティングポイント全て（同一平面であって連続した領域に含まれるとされる距離データ点群）が含まれる領域を示し、図 16Bは、図 16Aに示す図形から求めた凸多角形である。ここで示す凸多角形は、与えられた平面図形 (サポーティングポイントが含まれる領域)を含む最小の凸集合を求める凸包（convex hull)を利用したものとすることができる。なお、 Gで示す点は、後述するが、踏面の幅 Wを求める際に使用するもので、例えばサポーティングポイントが含まれる領域の重心などの点 (参照点)を示す。

このような凸包を利用して凸多角形を求めるアルゴリズムの例として、上述したように、 Melkmanのアルゴリズム、 Sklanskyのアルゴリズムなどがある。図 17は、 Melkmanのアルゴリズムを説明するための模式図である。図 17に示すように、与えられた図形に含まれる点を 3点 PI, P2, P3抽出し、点 PI, P2を結ぶ線分を引き、点 PI, Ρ3、 , P 2, P3を通る直線を引く。これにより、 3点 PI, P2, P3からなる三角形 AR4を含む 5 つの領域 AR1— AR5に区画される。そして、次に選択した点 P4がどの領域に含まれるかを判断して多角形を形成しなおすという処理を繰り返して凸多角形を更新していく。例えば P4が、領域 AR1に存在する場合、 PI, P2, P4, P3の順序で結んだ線分に囲まれた領域が更新された凸多角形となる。また、領域 AR3, AR4に点 P4が存在する場合には、それぞれ PI, P2, P3, P4の順序で結んだ線分に囲まれた領域， PI, P4, P2, P3の順序で結んだ線分に囲まれた領域として凸多角形を更新する。一方、領域 AR4、すなわち凸多角形内部に点 Pが含まれる場合は、凸多角形は更新せず、また、領域 AR2に点 P4がある場合は、点 P3は除き PI, P2, P3の順序で結んだ線分に囲まれた領域として凸多角形を更新する。本実施の形態においては、全てのサポーティングポイントに対して、各点に含まれる領域を考慮して凸多角形を生成することができる。

図 18は、 Sklanskyのアルゴリズムにより多角形を求める方法を説明するための模式図である。 Sklanskyのアルゴリズムにより抽出される多角形は、 Weakly Externally Visible Polygonと呼ばれるものである力上述の Sklanskyのアル比して計算量が少なぐしたがって高速演算が可能である。

与えられた図形を包含する凸多角形を求める場合、図 18Aに示すように、与えられた図形 131の境界上の任意の点 Xから図形 131を含む円 132に対して半直線を引く。このとき、点 Xから円 132に引いた半直線のうち、図形 131を横切らない半直線を引くことができた場合、この点は凸多角形の境界を構成する点であるとする。一方、図 1 8Bに示すように、与えられた図形 133の境界上任意の他の点 yから図形 133を含む円 134に半直線を引いた場合には、図形 133を横切らない半直線を引くことができない。この場合は、この他の点 yは凸多角形の境界を構成しないものとする。以上のようにして、順次各点が凸多角形の境界を構成する力否かを判断して選択された点のみからの図形を求めると、図 16Aに示すような図形が得られる。

この図形を包含する凸多角形を求めることで、図 16Bの凸多角形を得ることができる。ここで、本実施の形態においては、ステレオビジョンシステム 1の精度、特性などを考慮し、図 16Aから凸多角形を求める場合には、図 16Bに示すように、図 16Aの図形に外接する凸多角形を求めるものとして説明するが、カメラの精度、特性などを考慮し、例えば図 16Aの図形に内接する凸多角形を求めるようにしてもよいことは勿論である。また、これらの方法を平面の傾き度合いや、周囲の状況に応じて使い分けるようにしてちょい。

また、図 15のステップ S2において凸包によって入力平面を包含する多角形を求めた場合、非凸多角形形状の階段について問題が発生する。図 19は、この問題を示す模式図であって、図 19Aは、入力される平面であり stepOは非凸多角形形状の階段である。図 19Bは、凸包による stepOの多角形表現結果であり、非凸部分に関して望ましい結果と大きな乖離が生じている。このような非凸多角形を扱う方法として、ギヤップ除去とラインフィットによる平滑ィ匕によって多角形を求める方法が考えられる。ギャップ除去とラインフィットによる平滑ィ匕によって入力平面を包含する多角形を求める方法について説明する。図 20は、平滑ィ匕を示す模式図であって、図 20Aは、入力された一の平面に属すると判断されたサポーティングポイント全て（同一平面であつて連続した領域に含まれるとされる距離データ点群）が含まれる領域である入力多角形（input polygon)を示し、図 20Bは、入力平面を示す多角形から不連続なギヤップを除去し（close gaps)平滑化した多角形（ギャップ除去多角形： gaps closed polygon)を示し、図 20Cは、図 20Bで得られた多角形に対してラインフィッティングにより（fit line segments)更に平滑化した多角形（平滑化多角形： smoothed polygon)である。ここで、図 21は、ギャップ除去とラインフィットによる平滑ィ匕によって入力平面を包含する多角形を求める処理のプログラム例を示す図であり、多角形力不連続なギャップを除去する Close gaps処理、及び得られた多角形に対してラインフイツティングにより更に平滑化する Fit line segments処理を示している。

まず、ギャップ除去の方法について説明する。多角形を示す頂点から連続する 3つの頂点を選び、この中央の点が端点を結んだ直線から大きく離れている場合にこの中央の点を除去する。残った頂点について、この処理を除去する点が無くなるまで続ける。次に、ラインフィットの方法について説明する。多角形を示す頂点から連続する 3つの頂点を選び、最小二乗法によりこれらの 3点を近似する直線とこの直線と 3点の誤差を求める。求められた全ての近似直線と誤差について、誤差の小さい順に並べ、誤差がある閾値より小さい場合に、中央の点を除去し、端点の位置を近似直線によつて再計算する。この処理を除去する点が無くなるまで続ける。

次に、図 15のステップ S3における処理について説明する。先ず、得られた多角形（図 16B、図 20C)力も上述した階段パラメータを算出する。図 22は、階段パラメータの算出方法を説明するための模式図である。図 22Aに示すように、得られた多角形 140力点 141一点 147により囲まれる領域であるものとする。ここで、ロボット装置 2 01からみて多角形 140の前側の境界を構成する線分がフロントエッジ FE、奥側の境界を構成する線分がバックエッジ BEである。

階段踏面の幅 Wは、フロントエッジ FEの中心点 C と参照点 Gとを結ぶ線分の長さ

FE

を dl、ノックエッジ BEの中心点 C と参照点 Gとを結ぶ線分の長さを d2としたとき、

BE

幅 W=dl + d2とすることができる。

ここで、参照点 Gは、踏面となる平面の略中心であればよぐ例えば、全サポーティングポイントの中心点としたり、多角形 140の重心としたり、フロントエッジ FE及びバックエッジ BEの端点を結んだ図 22Bに示す安全領域 152の重心としたりすることができる。

こうして得られたフロントエッジ FE、ノックエッジ BE、参照点 Gに基づき、階段の長さ L及び幅 Wを求めステップ S4の処理を実行する。階段の長さ Lは、フロントエッジ F E及びバックエッジ BEの長さのうち例えば短い方としたり、以下に示す左右のマージンを含めたフロントエッジ FE及び左右のマージンを含めたバックエッジ BEのうち長い方としたりすることができる。

次に、ステップ S5におけるマージン算出方法について説明する。図 23は、最終的に認識される踏面及び階段パラメータを説明するための模式図である。本実施の形態においては、図 22B、図 23に示すように、安全領域 152の左右端部にマージン M , Mを設け、この左右のマージン M , Mを含んだ領域 151を最終的に踏面として

1 2 1 2

認識するものとする。左右のマージン M , Mは、フロントエッジ FE及びバックエッジ BEにて規定される安全領域 152の外側に多角形がはみ出している場合、先ず、それらの点を選択する。図 22Aにおいて、例えば右マージン Mを求める場合であれば

2

、点 142， 143， 146である。安全領域 152の右側に隣接した右マージン Mを求める

2 際は、安全領域 152から最も離れた点である点 142を選択し、この点 142からフロントエッジ FE、バックエッジ BEに対して垂線を下ろす。そして、これら垂線及びフロントエッジ FE、ノックエッジ BEに囲まれる領域 151を踏面として認識するものとする。なお、このマージンの求め方としては、点 142を通り、フロントエッジ FE又はバックエツジ BEと交差する線分を引くのみでもよい。

ここで、図 22Bに示すように、フロントエッジ FEと同一直線における左マージン M の長さを lmfとし、バックエッジ BEと同一直線における左マージン Mの長さを lbmとする。同様に、フロントエッジ FE,バックエッジ BEとそれぞれ同一直線における右マ一ジン Mの長さを rfm, rbmとする。

2

このようにして多角形力もフロントエッジ FE、ノックエッジ BEを求めて階段を認識することの効果について説明しておく。図 24A及び図 24Bは、 2種類の階段を示す模式図である。図 24Aに示すのは、図 9、図 10に示したような踏面が長方形の形状の階段であるが、図 24Bは、螺旋 (spiral)状の階段である。この図 24Bのような螺旋階段の場合、フロントエッジ FEに対してバックエッジ BEは平行でない。したがって、例えば単に検出した平面力長方形領域を抽出してしまうようなアルゴリズムを適用できない場合がある。したがって、本実施の形態のように、検出した平面から多角形を求め、フロントエッジ FE及びバックエッジ BEを求めることにより、このような螺旋階段であってもロボット装置が昇降動作することが可能となる。

次に、図 14に示す階段統合器 6について説明する。階段統合器 6は、階段検出器 5によって検出された階段データ（階段パラメータ) D3を入力とし、それらの階段データ D3を時間的に統合することで、より正確かつ高域な階段の情報を推定するものである。例えば、ロボット装置の視野が狭い場合などにおいて、一度に階段全体を認識できない場合がある。そのような場合、例えば、例えば前フレームなどの古い階段データと例えば現フレームなどの新しい階段データとの中で、空間的にオーバーラップしている階段の組を探し、オーバーラップしている階段を統合することにより、新しい仮想的な階段を定義する。この作業をオーバーラップする階段がなくなるまで続けることによって正確な階段を認識することが可能となる。

図 25は、階段統合器 6における階段統合処理の方法を示すフローチャートである。まず、現在の階段データ（New Stairs)と古、階段データ（Old Stairs)とを入力とし (ステツプ SI 1)、新、階段データ及び古!、階段データの全てを 1つの集合 (ujnion)とする (ステップ S12)。これら合わされた階段データ集合において、空間的にオーバ一ラップしている階段データを検索し (ステップ S 13)、オーバーラップしている組がある場合には (ステップ S14 :Yes)、それらの階段を統合して当該階段データ集合に登録する（ステップ S15)。そして、ステップ S13、 14の処理を空間的にオーバーラップする階段の組がなくなるまで続け (ステップ S 14 : No)、最終的に更新された階段データ集合を階段データ D4として出力する。

図 26は、オーバーラップしている階段データを統合するステップ S13における処理を説明するための模式図である。図 26は、空間的にオーバーラップしている階段データ ST11, ST12を示す。空間的にオーバーラップしているかどうかの判断には、例えば 2つの階段データ ST11, ST12の参照点 Gにおける高さの差 (距離）と、左右のマージンを含めた踏面の領域が重なっている面積の大きさを利用することができる。すなわち、 2つの階段の重心 G , G の高さの差が閾値 (maxdz)以下であり、かつ、

11 12

オーバーラップして、る面積の大きさが閾値 (minarea)以上ある場合には、これらの 2 つの階段データが示す踏面はオーバーラップしていると判断することができる。その場合には図 26の下図に示すように、階段データ ST11, ST12を統合して重心 G の

13 階段データ ST13とする。

ここで、統合に際しては、階段データ ST11, ST12を含む外枠の領域をステップ S T13とし、統合前の階段データ ST11及び階段データ ST12の左右のマージンを除いた安全領域を含む領域を統合後の新たな安全領域 165とし、階段データ ST13からこの安全領域 165を除いた領域をマージン M , Mとする。統合後の安全領域 165

1 2

により統合されたフロントエッジ FE及びバックエッジ BEを求めることができる。

すなわち、統合後の階段データ ST13におけるフロントエッジ FEの両端点は、階段データ ST11のフロントエッジ FEと階段データ ST12のフロントエッジ FEの左右の端点を比較し、右端点 163はより右側にある方、左端点はより左側にある方とされる。また、フロントエッジ FEのラインの位置は、階段データ ST11のフロントエッジ FEと階段データ ST12のフロントエッジ FEを比較してよりロボット装置に近い方（手前側）のライン位置とされる。同様にバックエッジ BEもより奥側にある方の位置が選択され、より左右に広がるようにその左右の端点 161, 162が選択される。

なお、統合方法はこれに限るものではない。本実施の形態においては、ロボット装置の視界などを考慮してフロントエッジ FE及びバックエッジ BEにより決まる四角形の領域、統合データ ST13が共に最も大きくなるように統合するものとしている力例えば、十分視界が広い場合や、距離データの精度が十分高い場合においては、 2つの階段データを単に合わせた領域を統合後の階段データとするなどしてもょ、。また、統合後の参照点 Gは、階段データ ST11と階段データ ST12に含まれるサポーティングポイントの数の比に応じて重み付き平均をとつて求めることができる。

次に、図 7に示す階段昇降制御器 4について説明する。階段昇降制御器 4は、階段検出器及び階段統合器 6によって統合されて検出された階段データ D4を用、て実際にロボット装置が階段の昇降動作を行うための制御を行う。この昇降制御には、階段を探す動作も含むものとする。

本実施の形態において実現した階段昇降動作は、次の 5つのステートマシンとして構築することができる。

4-1：サーチ（Search)動作

ロボット装置の頭部に搭載されたステレオビジョンシステムにて距離画像を取得するため、首を振って周囲を見廻し、環境の情報を集める。

4 2 :ァライン (Align)動作

階段に対して正対し、かつ、決められた一定量の距離位置に移動する。図 27は、ァライン動作を説明するための図である。図 27において、領域 170がロボット装置の認識した階段踏面の 1段目であるとする。ァライン動作においては、踏面 170のフロントエッジ FEの中心点力も直交する方向に所定距離 ad (align.distance)離れた目的とする位置（以下、ァライン位置という。）に移動する。この場合、現在のロボット装置の位置が点 171であって、目的とするァライン位置が点 172である場合、両者の距離が所定の閾値 max_d以上離れている場合及びフロントエッジ FEに直行する方向と口ボット装置の向く方向との角度差が所定の閾値 ma_X__a以上ある場合、ロボット装置は目的とするァライン位置 172に移動を開始し、これらの条件を満たしたとき、ァライン動作が完了したと判断するものとする。これらの閾値としては、例えば max_d= 3 (cm) 、max_a = 2° とすることができる。

4-3：アプローチ（Approach)動作

アプローチ動作では、ロボット装置が階段の直前まで移動する。図 28は、ァプローチ動作を説明するための模式図である。図 28に示すように、階段と正対し

Align_distanceだけ離れた目的位置であるァライン位置 172に移動してァライン動作を終了したロボット装置 201は、階段の昇降を行うために、踏面 170のフロントエッジ FEの中心点 C とロボット装置 201とが正対し、かつその距離が所定の値 ax (

FE

approach.x)となる目的の位置（以下、アプローチ位置という。 )に移動する。

4-4 :クライム（Climb)動作

階段認識によって得られた階段データを元に階段昇降動作をする。次の踏面 (段）に移動した場合であって、次の段の踏面が観測されている場合には、更に昇る又は下る動作を続ける。この動作を次の段がなくなるまで続けて行うことにより階段昇降動作が実現される。

4-5：フィニッシュ（Finish)動作

クライム動作にて階段を登る動作をした場合は、最上段にいることを確認し、最上段の中央へ移動する。クライム動作にて降りる動作をした場合、又は図 9に示すような階段 ST1である場合には例えば向きを変えるなどして降りる動作をした場合には、床面に降りたことを認識することで階段昇降動作は終了される。

以上の階段昇降処理の動作方法について更に詳細に説明する。図 29は、階段昇降動作の手順を示すフローチャートである。図 29に示すように、階段昇降制御の処理が開始されると、サーチ（Search) 'ァライン (Align) 'アプローチ (Approach)動作によって、階段を検索し、検索した階段に対して対畤した所定位置に移動 (ァライン)し、階段の 1段目に近づくアプローチ動作を実行する (ステップ S21)。このサーチ'ァライン'アプローチ動作が成功した場合 (ステップ S22 : Yes)、後述する方法にて階段昇降を行い (ステップ S23)、成功したことを出力する。近づくことに失敗した場合 (ステツプ S22 : No)、失敗したことを出力して処理を終了する。この場合は、もう一度、ステツプ S21の処理力繰り返すなどする。

ここで、ステップ S21におけるサーチ 'ァライン ·アプローチのシーケンスは通常ロボット装置が物体や目的地に到達するために用いる制御と同じである。具体的には例えば以下に示すような方法がある。図 30は、サーチ'ァライン'アプローチ処理方法を示すフローチャートである。

図 30に示すように、サーチ 'ァライン'アプローチが開始されると、サーチ動作（1)を実行する (ステップ S32)。サーチ動作（1)では、首を振りできるだけ広い範囲の情報を集める動作とする。その結果、周囲に昇降可能な階段があるかどうかを判断する（ステップ S32)。ここで、検出された階段の中で 1段目の踏面を構成する平面の高さ n を利用し、高さが step_min_z<n < step_max_zを満たす場合に、昇降可能であると判断する。昇降可能な階段があった場合 (ステップ S32 : Yes)、階段を近くで認識しなおすために、階段に対して決められた距離 (align_distance)まで移動するァライン動作を実行し (ステップ S33)、昇降しょうとしている階段を再度認識する (ステップ S34) 。このステップ S34の動作がサーチ動作（2)である。

そして、昇降可能な階段である力否力を再度確認し (ステップ S35)、サーチ動作（ 2)が成功している場合には、再認識した階段に対して対畤しかつ所定の距離のァライン位置に移動完了できている力否力すなわちステップ S33のァライン動作が成功している力否かを確認し (ステップ S36)、昇降可能な階段があり、かつァラインされている場合には（ステップ S35、 S36 : Yes)、初段の階段のフロントエッジまで進むァプローチ動作を実行する (ステップ S37)。一方、ステップ S35にて昇降可能な階段がない場合にはステップ S31の処理に戻り、ステップ S36にてァライン動作が成功して Vヽな、場合にはステップ S33の処理力も繰り返す。

次に、ステップ S22における階段昇降動作について説明する。階段昇降動作は、一段上又は下の段 (以下、次段という。）を現在の踏面力ロボット装置自身が認識できる場合の昇降動作処理 1と、現在の移動面から次段は認識できない場合等であつて、 2段以上、上の段又は下の段（以下、 2段以上先の段という。）を認識できる場合の昇降動作処理 2と、複数段の踏面が認識可能な場合の昇降動作処理 3とがある。図 31、図 33、図 34は、それぞれ昇降動作処理 1一 3の処理方法を示すフローチヤートである。ここで、以下の説明においては、現在移動中の段 (床面を含む）を step-0 、次段の段を step- 1、その更に次の段を step- 2、 m段先の段を step-mとする。

先ず、次段 (st印- 1)の踏面を観測 ·認識できる場合の昇降動作処理にっ、て説明する。図 31に示すように、昇降動作処理 1が開始されると、階段を登る Z降りる動作（クライム動作（1) )を実行する (ステップ S41)。このクライム動作（1)では、上述のステップ S32において、階段の高さ nを認識できているため、この高さ nの正負の判断を z z

することにより、昇降モードを切り替える。すなわち、 n < 0であれば降りる動作であり z

、 n >0であれば登る動作となる。登る場合と降りる場合では、後述するように、クライ z

ム動作に使用する制御パラメータの値が異なる。すなわち、制御パラメータを切り替えるのみで、登る動作と降りる動作を切り替え実行することができる。

そして、クライム動作（1)が成功した力否かを判断し (ステップ S42)、成功した場合 (ステップ S42 : Yes)は、サーチ動作（3)を実行する（ステップ S43)。このサーチ動作（3)は、ステレオビジョンシステムが搭載された頭部ユニットを動かし、周囲の距離データを取得して次の階段を検出する処理であり、サーチ動作（2)などとほぼ同様の動作処理である。

そして、ステップ S35と同様、サーチ動作（3)が成功したら (ステップ S44 : Yes)、再びステップ S41の処理を繰り返す。階段が検索されな力つた場合 (ステップ S44 : No )、階段を登り切ったか、又は下りきつたものと判断し、階段面の中央など所定の位置まで移動するなどのフィニッシュ動作を実行し (ステップ S45)、処理を終了する。次に、ステップ S41のクライム動作（1)について詳細に説明する。図 32は、ロボット装置が認識しているか又は認識する予定の階段面を示す模式図である。図 32に示すように、例えば、現在移動中のロボット装置の足底 121LZRが踏面 181にあるとする。また、この図 32においては、フロントエッジ FE及びバックエッジ BEに挟まれる安全領域及びこれに隣接する左右のマージン M , Mを踏面と認識することとする。こ

1 2

のとき、ロボット装置は、次の次段（step- 2)の踏面 182を認識できている。ここで、踏面 181と踏面 182との間にはその蹴り上げなどによりギャップ 184が存在するものとする。昇降動作（1)では、現在の段 (st印- 0)の踏面 181から次の段 (st印- 1)の踏面 182に移動 (クライム動作)可能であるかを判断するが、例えば、以下の基準を満たすものを移動可能と判断するものとする。

5—1 :次段（step-1)の踏面 182のフロントエッジ FEに十分近ぐ角度のずれが所定の閾値以下

5-2：次段 (step-1)の踏面 182の大きさが十分に大きヽ

5— 3 :フロントエッジ FE力も足底 121LZRの後端までの距離 front_xが指定された昇降モードにおける制御パラメータ front_x_limitより大きい

5— 4 :バックエッジ BE力も足底 121LZRの前端までの距離 back_xが昇降モードにおける制御パラメータ back_x_limi1:より大き!/ヽ

ここで、更に次段 (step-2)の踏面 183を観測できた場合、次段 (step- 1)の踏面 18 2の参照点における高さ zlと更に次の段（step- 2)の踏面 183の参照点における高さ z2の差（z2— zl)から、次段（step-1)の踏面 182からその次の段（step-2)の踏面 18 3へのクライム動作が登りであるか下りであるかを判断することができる。なお、 2段先の段 (st印- 2)の踏面 183が認識できない場合は、現在の昇降状態が維持されるものとすればよい。

上記の 5— 1一 5— 4力もクライム動作可能と判断した場合は、階段を登る又は降りるクライム動作を実行する。ロボット装置は、現在の段 (step- 0)の踏面 181において、踏面 181のバックエッジ BEにァラインしている。ここで、踏面 181と踏面 182との間

0

のギャップ 184が大きい場合には、次段（step-1)の踏面 182のフロントエッジ FEにァラインする動作をした後に、次段に移動し、そのノックエッジ BEにァラインする。そして、次のクライム動作にて、次の段（step-2)の踏面 183のフロントエッジ FEにァラ

2 インし、踏面 183に移動し、そのバックエッジ BEにァラインする。すなわち、例えば、

2

次の段の踏面のフロントエッジ FEにァラインして、昇降動作し、その移動した踏面のバックエッジ BEにァラインするまでをクライム動作とする。

また、ギャップ 184が小さい場合には、現在の段（step- 0)の踏面 181のバックエツジ BEと次段の踏面 182におけるフロントエッジ FEがほぼ一致するものとして、何れ

0 1

か一方のエッジにのみァライン動作するようにしてもよい。すなわち、現在の踏面 181 のバックエッジ BEにァラインして次段の踏面 182に移動し、そのバックエッジ BEに

0 1 ァラインする力、次段の踏面 182のフロントエッジ FEにァラインして次段の踏面に移動し、更に次の段の踏面 183のフロントエッジ FEにァラインすればよい。この場合、クライム動作とは、次段のフロントエッジ FEにァランする処理を省略し、次段に移動して移動した踏面のバックエッジ BEにァライン動作を実行する処理となる。

以上の昇降動作処理 1は、ロボット装置が階段を昇降動作中に次に移動可能な段 (step-1)の踏面を観測できる場合に適用することができる。例えば 2足歩行ロボット装置であれば、自身の足元を見下ろすことが可能なステレオビジョンシステム 1を搭載しておく必要がある。

次に、次段の踏面を観測 ·認識可能な場合の昇降動作処理 1とは異なり、ロボット装置の頭部ユニットと体幹部ユニットとの接続部の可動角の制約などにより、現在の段（ step- 0)における踏面力次段 (step- 1)の踏面を観測.認識することができず、 2段先 (step- 2)又はそれ以上先の段 (step-m)の段の踏面を認識可能な場合の昇降動作処理 2、 3について説明する。先ず、 2段先の段 (st印- 2)の踏面を認識可能な場合について説明すると、図 32に示すように、昇降動作処理 2が開始されると、上述と同様に、サーチ動作 (4)を実行する (ステップ S51)。このサーチ動作 (4)では、ステレオビジョンシステム 1により 2段先の段 (st印- 2)の踏面を観察'認識する。このサーチ動作 (4)は、 2段先の段 (step- 2)の踏面を認識する以外、上述のステップ S43などと同様の処理である。

そして、クライム動作 (2)を実行する (ステップ S52)。クライム動作 (2)は、クライム動作（1)と同様の動作である。この場合においても、クライム動作における階段を登る動作と降りる動作の切り替えは、同じく次段 (step-1)の踏面の高さ nにより判断すること z

ができる。次段 (step- 1)の踏面とは、現在の段 (step-0)の踏面より時間的に前に移動して、た段の踏面にぉ、て観測された踏面である。

そして、クライム動作が成功した場合 (ステップ S53 : Yes)、ステップ S51にてサーチした結果、 2段先の階段 (step- 2)が検出されていれば (ステップ S54 : Yes)、それを次の階段として更新し（step-1 = step- 2) (ステップ S56)、ステップ S51からの処理を繰り返す。ステップ S51のサーチ動作 (4)にて 2段先の段 (step- 2)の踏面が検出されていない場合 (ステップ S55 : No)、フィニッシュ動作を実行し (ステップ S55)、処理を終了する。

なお、ここでは 2段先の段の踏面を観測できることとして説明したが、 3段以降先の踏面を観測できる場合であっても同様にして処理すればょ、。

次に、複数段 (以下、 m段)先までの踏面が観測'認識可能な場合についの昇降動作を昇降動作 3として説明する。

図 34に示すように、 1一 n段が観測'認識できている場合に昇降動作を行う場合、まず、サーチ動作（5)を実行する。サーチ動作（5)は、基本的には、ステップ S51と同様であるが、認識可能な m段先の段 (st印- m)までの踏面を観測対象として、る点が異なる。

そして、クライム動作 (3)を k段分実行する (ステップ S62)。クライム動作 (3)においても、現在までに観測されて、る複数の踏面の高さの差力も昇降モードを決定することができる。すなわち、潘目、ト 1番目の踏面の高さ z— z

i i-1が負であれば階段を下りる動作となり、 0又は正であれば階段を登る動作モードとすればよい。このクライム動作（3)において移動する踏面の情報は、現在の段より時間的に m段前にて観測済みのデータである。

そして、昇降動作が成功した場合 (ステップ S63 :Yes)、現在の踏面より先の階段データが観測できたか否力すなわち m+n>kであるか否かが判定され (ステップ S 64)、 m+n>kであれば（ステップ S64 :Yes)、 step- (k+1)— step- (n+m)を、 step- 1— step-(m+n— k) (n=m)として、更新し (ステップ S66)、ステップ S61からの処理を繰り返す。一方、 mく 0であれば (ステップ S64 : No)、次に移動する対象となる踏面が存在しないため、フィニッシュ動作を実行し (ステップ S65)、処理を終了する。

以上説明したように、階段昇降動作処理 1一 3においては、クライム動作における登る動作と降りる動作とで使用する制御パラメータを変更するのみで、階段を登ることも、降りることも同様の手順にて実行することができる。階段昇降動作に使用する制御パラメータは、ロボット装置の足底の現在の踏面に対する位置を規制するためのものである。

図 35Aは、ロボット装置により認識されている踏面と足底の関係を説明するための図、図 35Bは、クライム動作に使用する制御パラメータの一例を示す図である。

図 35Aに示す各制御パラメータは、以下を示す。

step min.z：現在の段と次段の段との高さの差 (蹴り上げ)の昇降可能最小値 step max.z：現在の段と次段の段との高さの差 (蹴り上げ)の昇降可能最大値 ad (align.distance)：ァライン位置におけるフロントエッジ FEとロボット装置との間の距離

ax (approach.x)：アプローチ位置におけるフロントエッジ FEとロボット装置との間の距離

front_x_limit:踏面におけるフロントエッジ FEと足底 121の後端部との距離の限界値 (.minimal x- value)

back_x_limit：踏面におけるバックエッジ BEと足底 121の前端部との距離の限界値（ maximal x- value)

back_x_desired:バックエッジ BEと足底 121の前端部との距離の欲求値 (desired value;

階段を登る場合には、蹴り上げが _St_ep_min__Zより小さい場合は段差がある（階段）と見なさないものとし、蹴り上げが p_ma_X__Zより大きい場合には、階段昇降不可と判断する。同様に、階段を降りる場合には、蹴り上げが _Step_m_aX__Zより大きい場合には、階段と見なさないものとし、 p_min__Zより大きい場合には、階段昇降不可と判断するものとする。

aligi iistanceは、ァライン動作をする場合にのみ使用するパラメータであり、階段を登る Z降りる動作を実行する階段昇降動作処理を開始する場合、すなわち、初段の段に対する昇降動作を行う場合に使用される。同様に、 appr_0ach_xも、アプローチ動作をする場合にのみ使用するパラメータであり、階段を登る Z降りる動作を実行する階段昇降動作処理を開始する場合に使用される。

front_x_limit及び back_x_limitは、ロボット装置が認識している踏面と、ロボット装置の足底の関係を規定するもので、踏面のバックエッジ BE及びフロントエッジ FEと、足底の端部との距離、すなわちその踏面に移動した場合における踏面のあまり部分がこれらの値より小さい場合には、その踏面に移動できない、又は移動できたとしても次の昇降動作が不能であると判断される。ここで、登り動作において、 front_x_limit及び back_x_limitの値が何れも負であることは、踏面が足底より小さ、ことを許すことを示す。すなわち、登り動作においては、踏面は足底より小さい場合であっても移動可能と半 U断することができる。

back_x_desiredは、現在の踏面におけるバックエッジ BEに対してロボット装置がァラインしたい位置におけるバックエッジ BEと足底前端部との距離を示すものであり、図 35Bに示すように、通常、登る場合であれば、 back_x_desiredは、ノックエッジ BEよりも手前、本実施の形態においては、バックエッジ BEから 15mm手前の位置となっており、一方、降りる動作であれば、ノックエッジ BEより足底がはみ出した位置、本実施の形態においては、 5mmはみ出した位置となる。これは、登る動作であれば、次段に足上げして移動するまでにある程度の距離が必要であるのに対し、降りる動作においては、そのような距離が不要であると共に、踏面からはみ出すような位置の方が次段又はそれ以降の踏面の観測 ·認識が容易となるためである。

図 36及び図 37は、図 35に示す制御パラメータを使用して実際にロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。図 36は、ロボット装置が階段を登る動作を示している。最上段から番号順に、ステップ S31のサーチ動作（1) 実行の様子 (No. 1)、ステップ S33のァライン動作実行の様子 (No. 2)、ステップ S3 2のサーチ動作（2)実行の様子 (No. 3)、ステップ S37のアプローチ動作実行の様子（No. 4)、ステップ S51のサーチ動作（4)実行の様子（No. 5)、ステップ S52のクライム動作（2)実行の様子 (No. 6)、ステップ S52のクライム動作（2)の続きであって現在の踏面のバックエッジ BEにァラインしている様子（No. 7)、ステップ S51のサーチ動作 (4)実行の様子 (No. 8)、 · · ·を示し、サーチ動作 (4)をして次段の踏面が観測されな力つた場合 (No. 17)に、階段昇降動作終了 (フィニッシュ)動作を行っている様子 (No. 18)を示す。

図 37は、降りる動作を示すものであり、図 36の登る動作と同様に、サーチ動作 (No . 1, No. 4、 No. 7、 No. 10、 No. 13、 No. 16)、クライム動作（ァライン動作含む）を繰り返し（No. 5、 No. 6、 No. 8、 No. 9、 No. 11、 No. 12、 No. 14、 No. 15)、次段の踏面が観測されなくなった時点でフィニッシュ動作を行って (No. 18)、昇降動作を終了する。

次に、本実施の形態における変形例について説明する。以上の説明においては、階段を登る動作、降りる動作について説明したが、本実施の形態の制御方法に係るアルゴリズムを適用すれば、複数の段力なる階段のみならず、 1段のみ力なる段部や、 1段の凹部が存在しても、その段差が所定の値以下であれば、移動を可能にするものである。

先ず、単一の段部に登る動作について説明する。図 38は、単一の段部とロボット装置の足底の関係を示す図である。図 38において、 191で示すのは、床面（z = 0)力もの高さが zl = 30である段部を示し、ここでは、ロボット装置が次段 (stepl)となる段部 191の紙面下側から上側に移動する場合にっ、て説明する。段部 191に対し紙面下側における移動面の高さが z0 = 0、紙面上側における移動面の高さが z2 = 0である場合であって、現在、高さ ζθの移動面を移動しているとした場合、 zl— z0 = 30 >0 であるので次段の段部 191への移動は登り動作と判断し、 z2— zl =— 30く 0であるので段部 191から次の領域への移動は降りる動作であると判断することができる。この判断に応じてクライム動作にお、て、上述した制御パラメータの値を変更すればよい。ここで、図 35Bに示す制御パラメータを使用した場合、登る動作の場合の ront_x_limit及び back_x_limitは、何れも負の値であって、ロボット装置の足底 121が図 38に示すように、段部 191からはみ出した状態であっても移動可能と判断することを示す。

次に、単一の窪み（凹部）に降りる動作について説明する。図 39は、単一の凹部とロボット装置の足底の関係を示す図である。図 39において、 192で示すのは、床面（ z = 0)力もの高さが zl =— 30である凹部を示し、ここでは、ロボット装置が次段（stepl )となる凹部 192の紙面下側力も上側に移動する場合について説明する。凹部 192 に対し紙面下側における移動面の高さが z0 = 0、紙面上側における異動面の高さが z2 = 0である場合であって、現在、高さ ζθの移動面を移動しているとした場合、 zl— z 0=— 30く 0であるので次段の凹部 192への移動は降りる動作と判断し、 z2-zl = 3 0 > 0であるので凹部 192から次の領域への移動は登る動作であると判断することができる。したがって、段部 191と同様、この判断に応じてクライム動作において、上述した制御パラメータの値を変更すればよい。ここで、図 35Bに示す制御パラメータを使用した場合、降りる動作の場合の ront_x_limit及び back_x_limitは、何れも正の値であって、ロボット装置の足底 121が図 39に示すように、凹部 191より小さい場合にのみ移動可能と判断する。

本実施の形態においては、検出された平面力も例えば水平など、移動可能と判断できる平面を抽出し、その領域を含む多角形力階段の踏面を認識する。そして、多角形のフロントエッジ FE及びバックエッジ BEなどの踏面に関する情報と、その床面からの高さを含む階段情報を利用して階段昇降動作を行う。昇降動作においては、移動する踏面をサーチ動作を実行し、サーチできた踏面のフロントエッジ FE又は現在の移動面におけるノックエッジ BEに対してァライン動作を実行し、次段の移動面と現在の移動面との高さの違いから登る動作か降りる動作かを判断して制御パラメータを切り替えることにより、通常の矩形の踏面力なる階段のみならず、スパイラル形状の階段などの昇降動作が可能となると共に、登る動作も降りる動作も制御パラメータを変更するのみで同一の手順にて実行することができる。したがって、階段のみならず、単一の段部や、単一の凹部などへの移動も同一の制御方法にて移動可能となるまた、階段認識においては、認識した階段を時間的に統合していくため、例えば口ボット装置の大きさに対して階段が大きい、ロボット装置に搭載されるステレオビジョンシステムの位置の制約などの理由で視野が限られたロボット装置などにぉ、ても、高域に亘つて階段情報を認識することができる。また、この階段情報を利用して昇降動作する際、同じくステレオビジョンシステムの位置の制約などの理由で次段の踏面が観測 '認識できない場合であっても、過去に観測 '認識してある階段情報を利用して昇降動作を同様に行うことができる。

本変形例における平面検出装置は、線分拡張法により、視野内において支配的な平面だけでなぐ例えば階段など複数の平面が存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさせることにより計測ノイズに対してロバストな平面検出結果を得ることができるものである。図 40は、本変形例における平面検出装置を示す機能ブロック図である。図 40に示すように、平面検出装置 100は、 3次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System) 1と、 3次元の距離データからなる距離画像に存在する平面を線分拡張法により検出する平面検出部 2とを有する。平面検出部 2は、画像を構成する距離データ点から同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部 2 aと、画像内に含まれる、線分抽出部 2aよって抽出された全線分力ゝらなる線分群から、該画像内に存在する 1又は複数の平面領域を検出する領域拡張部 2bとを有する。領域拡張部 2bは、線分群から同一平面上に存在すると推定される任意の 3本の線分を選択し、これらから基準平面を求める。そして、選択した 3本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新すると共に基準平面の領域を拡張する。

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

領域拡張部 2bは、この線分群 D11において、同一の平面に属すると推定される線分を 3本選択し、これら力も基準平面としての種となる平面を求める。この種となる平面の領域 (領域種: seed region)に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群 D2を出力する。

ロボット装置 201は、障害物回避や階段昇降など平面の情報が必要なとき、または定期的にこれらの処理を行うことによって、階段や床面、壁といった歩行に重要な平面の情報を取得する。

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

そこで、図 41に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステムを構成するステレオカメラ 11RZLを備えると共に、例えば同じく頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源 12を設ける。この光源 12は、模様がない階段 ST3、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射 (照射)し、ランダムなパターン PTを付与する模様付与手段として作用する。なお、ランダムパターン PTを形成して距離画像を取得できるものであれば、ランダムパターン PTを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載される CCDカメラなどにおいては観測可能なパターンを付与することができる。

次に、平面検出装置 100の平面検出部 2について説明する。この平面検出部 2は、線分拡張法を使用して平面を検出するものであり、図 42は、線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出では、図 42に示すように、まず、焦点 F力も撮影された画像 11において、行方向または列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row:イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分カゝらなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。

図 43は、線分拡張法による平面検出処理を示すフローチャートである。図 43に示すように、先ず、距離画像を入力し (ステップ S71)、距離画像の行方向（又は列方向 )の各画素列において同一平面に属すると推定されるデータ点力も線分を求める (ステツプ S72)。そして、これらの線分群の中から同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める（ステップ S73)。このステップ S73では、まず、平面の種となる領域 (以下、領域種 (seed region)という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の 1ラインを含む 3本の線分が同一平面にあることを条件とする。ここで、選択した 3本の線分力なる領域種が属する平面を基準平面とし、 3本の線分から平均して求まる平面を求めておく。また、 3本の線分からなる領域を基準平面領域とする。そして、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し (領域拡張処理)、追加した線分を含めたものとして上記基準平面を更新し (平面更新処理)、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域 (3 本の線分)が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結する。そして、本変形例においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップ S74として更に設け、最終的な平面とするが、詳細は後述する。

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

以下、図 43に示す線分拡張法による平面検出方法について更に詳細に説明する。線分抽出部 (Line Extraction)2aは、上述したように、ステレオビジョンシステム 1からの 3次元距離画像を入力とし、距離画像の各列または各行毎に 3次元空間内で同一平面上にあると推定される線分を検出する。この線分抽出において、計測ノイズなどによる、上し 7こ over— segmentationや under— segmentationのロ題、すなわ、本来複数の平面であるのに 1つの平面として認識してしまつたり、本来は 1つの平面であるのに、複数の平面として認識してしまったりする問題を回避するため、データ点の分布に応じて適応的に線分フィッティングさせるアルゴリズム（Adaptive Line Fitting)を導入する。 Adaptive Line Fittingは、線分抽出部 2aにおいて、先ず比較的大きい閾値を使用して大まかに第 1の線分としての線分を抽出し、次に抽出された第 1の線分に属するデータ点群力後述する最小二乗法によって得られる第 2の線分としての線分に対する該データ点群の分布を解析する。すなわち、同一平面上に存在するか否かを大まかに推定してデータ点群を抽出し、抽出したデータ点群におけるデータ点の分布の偏りがある力否かを解析して同一平面上に存在している力否かを再度推定する。

本変形例においては、このデータ点の分布を解析し、データ点群が後述するジグザグ形 (zig-zag-shape)に当てはまる場合には、分布に偏りがあるとしてデータ点群を分割する処理を行い、これを繰り返すことによって、データ点群に含まれるノイズに対して適応的に線分の抽出を行うアルゴリズムを使用するものとする。

図 44は、線分抽出部 2aにおける処理、すなわち、図 43におけるステップ S72の処理の詳細を示すフローチャートである。まず、線分抽出部 2aには、距離データが入力される。入力された距離データのうち、例えば行方向の画素列 (データ点列）において、 3次元空間上で同一平面上に存在すると推定されるデータ点群を抽出する。 3 次元空間上で同一平面上に存在すると推定されるデータ点群は、例えば隣接するデータ点の距離が、例えば 6cm以下など、データ点間の 3次元空間における距離が所定の閾値以下のものからなるデータ点の集合などとすることができ、これをデータ点群 (Ρ [0 · · ·η-1] )として抽出する (ステップ S81)。そして、このデータ点群 Ρ [0 · · · n-1]に含まれるサンプル数 nが処理に最低限必要なサンプル数 (必要最小値) min_n より多いか否かをチェックし (ステップ S82)、データ数 nが必要最小値 min_nより少ない場合 (S82 :YES)には、検出結果として空集合を出力して処理を終了する。

一方、サンプル数 nが必要最小値 min_n以上である場合（S82 :NO)、データ点群 P [0· · ·η-1]の一方の端点 Ρ [0]と他方の端点 Ρ [n-1]とを結ぶ線分 (弦) L 1を第 1の線分として生成する。そして、データ点群 Ρ[0· · ·η-1]から、この線分 L1との距離が最も大き、データ点を着目点 brkとして検索し、その距離 distを算出する (ステップ S83)。最大距離 distがデータ点群分割の閾値 ma_X_dより大きい場合には（S84 :YES)、デ一タ点群データ点群 Ρ[0· · ·η-1]を着目点（分割点） brkにて 2つのデータ点群 ρ [0· · •brk]及び P[brk' · ·η- 1]に分割する（ステップ S88)。

一方、最大距離 distがデータ点群分割の閾値 max_dより小さい場合には（S84 : NO )、データ点群 Ρ[0· · ·η-1]力後述する最小二乗法によって最適な線分の方程式 lineを求め（ステップ S85)、この方程式 lineが示す線分 L2を第 2の線分として生成する。そして、データ点群 Ρ[0· · ·η-1]がこの線分 L2に対して後述する Zig-Zag-Shape であるかどうかを調べる（ステップ S86)、 Zig- Zag- Shapeでない場合（S86 :NO)、得られた線分の方程式 lineを線分抽出結果リストに追加し (ステップ S87)、処理を終了する。

また、ステップ S86にお!/、てステップ S85で求めた線分が Zig-Zag-Shapeである判断された場合（S86 : YES)、上述のステップ S84と同様、ステップ S88に進み、ステップ S83において距離 distを求めた着目点 brkにてデータ点群を 2つのデータ点群 P [0· · 'brk]及び P[brk' · ·η- 1]に分割する。このステップ S88にて 2つのデータ点群が得られた場合には、それぞれを再帰的に再度ステップ S81からの処理を行う。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すなわち全てのデータ点群がステップ S87を経るまで処理を繰り返し、これにより、全ての線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群 Ρ[0· · · n-1]力ノイズの影響を排除し複数の線分力なる線分群を精度よく検出することができる。

なお、ステップ S83にてデータ点群 Ρ[0· · ·η-1]の端点を結ぶ線分 L1を生成するものとして説明したが、例えばデータ点群 Ρ[0· · ·η-1]の分布、性質など必要に応じてデータ点群 Ρ[0· · ·η-1]力も最小二乗により線分 L1を求めてもよい。また、本変形例においては、着目点 brkは、端点を結んだ線分 L1との距離が最大の点 1つとしている 1S 例えば、上記のように最小二乗により求めた線分との距離が最大の点としたり、距離がデータ点群分割の閾値 max_d以上のものが複数ある場合はそれら全ての点又は選択した 1つ以上にてデータ点群 Ρ[0· · ·η-1]を分割するようにしてもよい。

次に、ステップ S85における最小二乗による線分生成方法（Least-Squares Line Fitting)について説明する。ある n個のデータ点群 Ρ[0· · ·η_1]が与とき、データ点群に最もフィットした直線の方程式を求める方法を示す。直線の方程式のモデルを下記式（1)で表す。

[数 1] xco& +ysiaa + d = Q ... ) この場合、 η個のデータ点群 Ρ[0· · ·η-1]の 1点（x,y)において、直線方程式のモデルとデータ点との誤差の総和は下記式（2)で表すことができる。

[数 2]

^Eflt ⁼ L(^xi∞s + y_t sia + dy ...(2) データ点群に最もフィットした直線は、上記式（2)の誤差の総和を最小化することによって求められる。上記式（2)を最小にする α及び dは、データ点群 Pの平均及び分散共分散行列を用いて下記（3)のように求めることができる。

[数 3]

1 _! -25 一

^~ 2^tan ~ ~ ~ ' ^{d =} ~^[-^{x os +}y^sina) ---(3)

ただし、

s^ =∑(Xi -x)² =£

i i

s ) Oi一次に、ステップ S86におけるジグザグ形 (Zig-Zag-Shape)判別方法につ!、て説明する。この Zig- Zag- Shape判別では、ある n個のデータ点群 Ρ[0· · ·η- 1]と直線 Line , d)、 xcosa +ycosa +d=0力与えられたとき、そのデータ点群 Ρ[0· · ·η- 1]が、図 4 5Αに示すように直線 Lineに対して交差する力、図 45Bに示すように、例えばノイズなどの影響によりデータ点が一様に分布しているかを判別するものである。基本的には

、直線 Lineの一方にデータ点群 Ρ[0· · ·η-1]が連続して現れる数をカウントし、ある一定数を超えて連続して現れる場合には、 zig-zag-shapeであると判断することができる。図 45Aの場合には、データ点群 Ρ[0···η-1]によりよくフィットする直線 Lineを求めるためにデータ点群 P[i]を分割する必要がある。図 46は、 Zig-Zag-Shape判別方法を示すフローチャートである。

まず、データ点群 Ρ[0···η-1]と直線 Line ,d, σ )とを入力する (ステップ S90)。ここで、 σは、点列の標準偏差を示す。次に、この標準偏差 σが所定の閾値 th_ σより大きいか否かを判断する。この標準偏差 σが閾値 th_aより小さい場合 (ステップ S9 1:Νο)は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、判別を終了する。そして、標準偏差 σが閾値 th_ σより大き!/、場合のみ判別処理を継続する。次に、データ点群 Ρ[0· · ·η-1]のうちの最初のデータ点 Ρ[0]が直線のどちら側にあるかを sing(sdist(P[0]》によって判断し、この結果を valに代入すると共に valと

0 0 同じ側にあるデータ点の連続数をカウントするカウンタ（以下、連続点カウンタといい

、このカウント値をカウント値 countという。）のカウント値 countを 1に設定する（ステップ S92)。ここで、 sign(x)は、 Xの値の符号（+又は一)を返す関数であり、 sdist(i)は、 P[i] .xcosa+ P[i].ycos α+dとして計算された直線 Lineにおいて、 i番目のデータ点との正負の距離を示す。すなわち、 Valには、データ点 P[0]が直線 Lineのどちら側にあるか

0

で +又は一の符号が代入される。

次に、データ点をカウントするためのカウンタ（以下、データ点カウンタといい、このカウント値をカウント値 iという。）のカウント値 iを 1とする（ステップ S93)。そして、データ点カウンタのカウント値 iがデータ数 nより小さい場合 (ステップ S94: YES)、その次のデータ（以下、 i番目とする。）のデータ点であるデータ点 P[i]が直線のどちら側にあるかを sing(sdist(P[i]》によって判断し、この結果を valに代入する (ステップ S95)。そして、ステップ 92にて求めた valとステップ S95にて求めた valとを比較し、 valと val

0 0 とが異なる場合 (ステップ S96 : NO)、 valに valを代入し、連続点カウンタのカウント値

0

countに 1を代入し (ステップ S98)、データ点カウンタのカウント値 iをインクリメントして (ステップ S100)ステップ S94からの処理に戻る。

一方、ステップ S96において、 valと valとが同じ場合 (ステップ S96 : YES)、データ

0

点 P [i— 1]と P [i]は、直線 Lineに対して同じ側にあると判断され、連続点カウンタの力ゥント値 countを 1つインクリメントする（ステップ S97)。更に、連続点カウンタのカウント値 countが Zig-Zag-Shapeと判定されるための最小のデータ点数 min_cより大きいか否か判定し (ステップ S99)、大きい場合には（ステップ S99 : YES)、 Zig- Zag- Shapeと判断し、 TRUEを出力して処理を終了する。一方、連続点カウンタのカウント値 count が最小のデータ点数 min_cより小さい場合には（ステップ S99 : NO)、ステップ S 100 に進み、データ点カウンタのカウント値 iをインクリメントして（ステップ S 100)、ステップ S94からの処理を繰り返す。

そして、このステップ S94からの処理を、データ点カウンタのカウント値 iがデータ点数 nに到達するまで続け、カウント値 i> =nとなったところで、 FALSEを出力して処理を終了する。

このようなジグザク形判別処理によって、 n個のデータ点群 Ρ [0 · · ·η-1]と直線 Line ( a ,d) : xcos a +ycos a + d=0が与えられたとき、このデータ点群が直線 Lineに対して zig-zagに交差しているかどうかを判断することができる。これによつて、上述したように、ステップ S86にてデータ点群を分割するべき力どうかを判断することができ、最小二乗により求めた直線に対し、データ点群が zig-zagに交差していると判断した場合にはデータ点群を分割すべきと判断してステップ S88の処理へ進み、着目点 brkを分割点としてデータ点群を分割することができる。なお、上記ステップ S91—ステツプ S 100までの処理は図 47のように表現することも可能である。

また、このような Zig-Zag-Shape判別処理は、演算器のみならずノヽードウエアで行うことも可能である。図 48は、 Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。図 48に示すように、 Zig-Zag-Shape判別処理部 20は、 n個のデータ点群 P [0 • · ·η-1]が入力され、順次各データ点 P[i]が直線 Lineの何れ側に位置するかを判別し、その判別結果 Valを出力する方向判別部 21と、 1つ後のデータと方向判別部 21 の結果を比較させるための遅延部 22と、データ点 P[i]における方向判別結果 Valとデータ点 P[i— 1]における方向判別結果 Valとを比較する比較部 23と、比較部 23にお

0

いて Val=Valの場合に、カウント値をインクリメントする連続点カウンタ 24と、連続点

0

カウンタ 24のカウント値 countと最小データ点数格納部 26から読み出した最小データ点数 min__Cとを比較する比較部 25とを有する。

この Zig-Zag-Shape判別処理部における動作は以下のようになる。すなわち、方向判別部 21は、データ点群 Ρ[0· · ·η-1]力も最小二乗法により直線 Lineを求め、各データ点 P[i]と直線 Lineとの正負の距離を求め、その正負の符号を出力する。遅延部 2 2は、データ点 P[i— 1]の直線 Lineまでの距離に対する正負の符号が入力されると 1つ後のデータ点 P[i]の正負の符号が入力されるタイミングまでデータを格納する。比較部 23は、データ点 P[i]とデータ点 P[i— 1]の上記正負の符号を比較し、同じ符号である場合にはカウンタ 24のカウント値 countをインクリメントする信号を出力し、正負の符号が異なればカウント値 countに 1を代入する信号を出力する。比較部 25は、カウント値 countと最小データ点数 min_cとを比較し、最小データ点数 min_cよりカウント値 countが大きい場合には、データ点群 Ρ[0· · ·η-1]がジグザグであることを示す信号を出力する。

次に、図 40に示す領域拡張部（Region Growing) 2bについて説明する。領域拡張部 2bは、線分抽出部 2aによって得られた線分群を入力とし、それらの線分それぞれがどの平面に属しているかを点列の平面への当てはめ（Plane Fitting)により判断し、与えられる線分群からなる領域を複数の平面 (平面領域）に分離する。複数の平面に分離するために、以下の手法をとる。

先ず、与えられた線分群から、同じ平面上にあると推定される隣接する 3本の線分を検索する。この 3本の線分により求められる平面 (基準平面）平面の種となるものであり、この 3本の線分が含まれる領域を領域種（seed region)という。そして、この領域種に隣接する線分を順次、基準平面と同一平面上にある線分か否かを点列の平面への当てはめ（Plane Fitting)により判断し、隣接する線分が同じ平面に含まれると判断された場合には、この線分を領域拡大用の線分として領域種に追加してその領域を拡大すると共に、基準平面の方程式を上記領域拡大用の線分を含めて再度算出し直す。このような処理によって、全ての線分を何れかの領域 (平面）に配分する図 49は、領域拡張処理を説明するための模式図である。図 49に示すように、画像 30内に複数の平面力もなる階段 31が存在する場合、例えば太線で示す 32a— 32c の 3本の線分が領域種として選択されたとする。これら 3本の線分 32a— 32cからなる領域が領域種となる。先ず、この 3つの線分 32a— 32cにより 1つの平面 (基準平面） Pを求める。次に、領域種の最も外側の線分 32a又は 32cに領域種外にて隣接するそれぞれデータ列 33又は 34において、平面 Pと同一の平面である線分を選択する。ここでは、線分 33aが選択されるとする。次に、これら 4本の線分群からなる平面 P'を求め、基準平面 Pを更新する。次に、線分 34aが選択されれば、 5本の線分群からなる平面 P' 'を求め、平面 P'を更新する。これを繰り返すことにより、階段 31の 2段目の踏面が、破線で囲まれる平面 45として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像 30内から領域種となる 3つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる 3つの線分がなくなるまで図 43のステップ S3の処理を繰り返す。

次に、データ点群 Ρ [0 · · · η-1]力も構成される平面の方程式を推定する手法 (Plane Fitting)、これを使用して領域種を選択する方法（Selection of seed region)、領域種から領域を拡大していく領域拡張処理 (Region growing)、及び得られた平面方程式力も誤差が大きいものなどを除いて再度算出する後処理 (Post processing)について説明する。

3次元空間内の点 Pは P= (x , y , により表され、平面の方程式はその法線べタトル n (nx, ny, nz)と非負の定数 dによって下記式 (4)で表される。

画 xn_x + yn„ + zn_z + d = 0 - - - (4) ここで、ステレオカメラでは、焦点を通る平面を観測することができない、すなわち、平面は焦点を通らないため、 d≠0とすることができる。したがって、平面は、最小二乗法により下記式（5)に示す値を最小にする値として求めることができる。

[数 5] fit(n,d) = _i(pjn + d)²- ...(5) 最適解は n=m/ " m " , d=~l/■ m■として求まる。ここで、 ■ ·■は、ベクトルの大きさ、 mは、行列式によって連立一次方程式を解くクラメールの法則（Cramer's rule)を使

用して下記（6— 1)のように容易に得られる線形システムの解である。

[数 6]

A m = b -.-(6-1)

ここで、

^A¾∑PiPi . ^b=∑Pi --(6-2) この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であつても、上記式 (6— 2)に示す Aと bの値を更新するのみで、平面パラメータを再計算

することができる。更に、本変形例における線分抽出方法の場合は n個のデータ点群の 2つのモーメント（1次モーメント：平均、 2次モーメント：分散） E(p)、E(pp )が既知

T

であり、これらを使用して、下記（7)に示すように A, bを更新することができ、 n個のデータ点群における平面更新処理に拡張することができる。

[数 7]

A A + nE(pp^T) ， b<-b + «E(p) ·.イ ₇)

また、一度平面パラメータ n, dを算出すれば、求まった平面方程式から、 n個のデ

ータ点群の平面方程式からの外れ度合!、を示す平面方程式の 2乗平均誤差 (RMS

(root mean square) residual) (以下、 rmsという。）を下記式（8)により算出することが

できる。この場合も、 n個のデータ点の上記 2つのモーメントを使用して下記式 (8)を求めることができる。

[数 8] P广 ·Ρ„) = ∑(ρΓη + > the rms 〜₍₈₎ 上記（8)に示すように、各データ点が求めた平面上にあれば平面方程式の 2乗平均誤差 rms (p · ' ·ρ )は 0になる値であり、この値が小さいほど各データ点が平面によくフィットして！/、ることを示す。

次に、領域種 (seed region)を検索する方法及び領域種から領域を拡大すると供に平面を更新する方法について説明する。図 50は、領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。図 50に示すように、領域種の選択には、先ず、線分抽出の際に使用した行方向又は列方向のデータ列が隣接する 3つの線分 (1 , 1 , 1 )であって、互いの線分 (1 , 1 ) , (1 , 1 )における画素位置が上記デー

1 2 3 1 2 2 3

タ列とは直交する方向にて重複したものを検索する (ステップ S101)。各データ点は画像内における画素位置を示すインデックス (index)を有しており、例えば行方向のデータ列における線分である場合、このインデックスを比較して列方向にて重複しているか否かを比較する。この検索に成功した場合 (ステップ S102 : YES)、上記式（7 )を使用して上記 (6—1)を算出する。これにより、平面パラメータ n, dを決定でき、これを使用して上記式 (8)に示す平面方程式の 2乗平均誤差 (1 , 1 , 1 )を計算する (ス

1 2 3

テツプ S103)。そして、この平面方程式の 2乗平均誤差 rms (l， 1， 1 )が例えば lcm

1 2 3

などの所定の閾値 th 1より小さい場合には、この 3つの線分を領域種として選択す rms

る（ステップ SI 04)。所定の閾値 th 1より大きい場合には、再びステップ S 101に戻 rms

り、上記条件を満たす線分を検索する。また、領域種に選ばれた線分は、線分群のリストから除くことで、他の平面拡張などの際に使用されないようにしておく。

こうして選択された領域種から線分拡張法により領域を拡張する。すなわち、先ず、領域種の領域に追加する候補となる線分を検索する (ステップ S 105)。なお、この領域は、領域種が既に更新されている場合の、後述する更新された領域種も含む。候補となる線分は、領域種の領域に含まれる線分 (例えば 1 )

1 に隣接する線分 (1 )

4であつて、上述同様、これらの線分の画素位置が相互に重なりあうことを条件とする。検索が成功した場合 (ステップ S 106 : YES)、その平面方程式の 2乗平均誤差 rms (1 )を算出し、これが所定の閾値 th 2より小さいか否かを判定し (ステップ S 107)、小さい rms

場合には平面パラメータを更新し (ステップ S 108)、再びステップ S105からの処理を繰り返す。ここで、候補となる線分がなくなるまで処理を繰り返し、候補となる線分がなくなつたら（ステップ S106 :NO)、ステップ S101の処理に戻り、再び領域種を検索する。そして、線分群に含まれる領域種がなくなった場合 (ステップ S 102 : NO)、今まで得られて、る平面パラメータを出力して処理を終了する。

ここで、本変形例においては、領域種を検索し、 3つの線分が同一平面に属するか否かの判定、及び領域拡張処理を行う際に基準平面又はこれを更新した更新平面に属する力否かの判定には、上記式 (8)を使用する。すなわち、平面方程式の 2乗平均誤差 rmsが所定の閾値 (th_rms)未満である場合にのみその線分 (群）を同一平面に属するものと推定し、その線分を含めた平面として再び平面を算出する。このように平面方程式の 2乗平均誤差 rmsを使用して同一平面に属する力否かを判定することにより、更にノイズにロバストでかつ、細かい段差を含んでいるような場合にも正確に平面を抽出することができる。以下にその理由について説明する。

図 51は、その効果を示す図であって、端点と直線との距離が等しくても平面方程式の 2乗平均誤差 rmsが異なる例を示す模式図である。ここで、非特許文献 4のように、領域拡張処理する際、注目の直線 (線分)の端点 (end point)と平面 Pとの距離 Dの値が所定の閾値より小さい場合に、当該注目の線分が平面 Pと同一平面であるとして領域拡張処理を行うと、平面 Pに交差する直線 La (図 51A)と、平面 Pと平行で所定距離ずれているような直線 Lb (図 51B)とが同様に平面 Pの更新に使用されることとなる。ここで、平面方程式の 2乗平均誤差 rmsを求めると、図 51Aの直線 Laから求まる平面方程式の 2乗平均誤差 rms (La)に比して図 51Bの直線 Lbから求まる平面方程式の 2乗平均誤差 rms (Lb)の方が大きい。すなわち、図 51 Aのように、直線 Laと平面 Pとが交差する場合は、平面方程式の 2乗平均誤差 rmsが比較的小さくノイズの影響である場合が多いのに対し、図 51Bのような場合、平面方程式の 2乗平均誤差 rm sが大きぐ直線 Lbは平面 Pと同一平面ではなく異なる平面 P'である確率が高い。したがって、複数の平面が含まれるような環境力平面を精確に求める必要がある場合などにおいては、本変形例のように、平面方程式の 2乗平均誤差 rmsを算出し、この値が所定の閾値未満である場合に同一平面と判断することが好ましい。なお、環境や距離データの性質に応じて、従来と同様、線分の端点と平面との距離が所定の閾値以下の場合は当該線分を平面に含めるようににたり、これらを組み合わせてもよいまた、面パラメータ n, dを一旦算出すれば、平面方程式の 2乗平均誤差 rmsは、データ点群について線分抽出の際に求めた 2つのモーメントの値から平面方程式を更新し、上記式 (8)にて簡単に算出することができる。

また、上述の領域種の選択方法は、図 52のようにも表現することができる。 overlapQ , 1 )は、各イメージロウに含まれる直線ベクトル 1と 1における端点間の位置が直線べ j k j k

タトルとは直交する位置にて重なっている場合に trueを出力する関数である。また、 fitPlaned , 1 , 1 )は、上記式（4)一（7)により Am=bの解を求め平面パラメータ n, dを

1 2 3

計算し、上記式 (8)により算出された A, b〖こより、直線ベクトル 1 , 1 , 1を平面にフイツ

1 2 3

ティングさせる関数である。

rms(l , 1 , 1 )は、上記式（6)を使用して 3本の直線全てにおいて、平面方程式の 2

1 2 3

乗平均誤差 rmsの値を算出する関数である。また、 removed , 1 , 1 )は、 lines [i] , lines

1 2 3

[i+l]l , lines [i+2]から領域種を構成するとして選択されたそれぞれ直線 1 , 1 , 1を除

2 1 2 3 くことを意味し、これにより、再びこれらの直線が計算に使用されることを防止する。また、領域拡張処理は、図 53のように表現することもできる。図 53において、 A及び bは、上記式（6— 1)に示すそれぞれ行列及びベクトルである、また、 add(A, b, 1)は、上記式（8)により、 Aと bに直線 lineのモーメントをカ卩える関数である。 Solve(A, b)は、 Am=bを満たす mを求め、上記式 (4)一（7)により平面パラメータ n, dを計算する。 select(open)は、例えば最初の 1つなど、 openの中力任意に 1つのエレメントを選択する関数である。また、 index(l )は、画素列又は行における 1のインデックス

1 1 を返す関数である。また、 neighbor(index)は、与えられたインデックスに隣接したインデックス、例えば {index- 1, index+1}を返す関数である。

また、上述したように、本変形例においては、図 43のステップ S73において領域拡張処理を行って平面方程式を更新した後、ステップ S 74にお、て平面方程式を再度算出する処理 (Post processing)を行う。この再度算出する処理では、例えば上述のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離データ点又は線分の平面力のずれを計算し、所定の値以上平面から外れる距離データ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低減することがでさる。

次に、このステップ S74について詳細に説明する。ここでは、 2つのステップにより、平面方程式を再度算出する方法について説明する。先ず、ステップ S73にて検出された各平面の境界の距離データ点（pixels)において、現在属している平面よりも、隣接する平面までの距離が近いデータ点が検出された場合は、当該データ点を隣接する平面の方に含める処理をする。また、何れの平面にも属していなぐかつ距離が例えば 1. 5cmなど比較的大きい閾値以下である平面が存在するデータ点が検出できた場合は、当該データ点をその平面に含める処理をする。これらの処理は各平面領域の境界近傍のデータ点を検索することで実行することができる。以上の処理が終了したら、再度平面方程式を算出する。

次に、上述のようにして再度算出された平面の各領域の境界近傍において、各データ点と平面との距離が例えば 0. 75cmなど比較的小さい閾値を超える場合は、それらのデータ点を捨てる処理を実行する。これにより、その平面領域は若干小さくなるものの更に精確な平面を求めることができる。距離データ点を削除後、再び平面を求め、この処理を繰り返す。このことにより、極めて精密に平面を求めることができる。次に各処理によって得られる結果を示す。図 54Aは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図 54Bは、縦軸を x、横軸を y、各データ点の濃淡で z軸を表現して 3次元距離データを示す図であり、更に、行方向の画素列力線分抽出処理にて同一平面に存在するとされるデータ点群力も直線を検出したものを示す。図 54Bに示す直線群から領域拡張処理によりえられた平面領域を図 54 Cに示す。このように、ロボット装置の視野内には、 1つの平面（床面）のみが存在する、すなわち、床面が全て同じ平面として検出されていることがわかる。

次に、床面に段差を一段置いたときの結果を図 55に示す。図 55Aに示すように、床面 Fには、 1段の段差 ST3が載置されている。図 55Bは、実験条件を示す図であり、着目点と直線 (線分)との距離力 ¾ax_dを超える場合は、データ点群を分割する。また、抽出の成否（水平）（correct extraction(horizontal))は、行方向のデータ列毎に、合計 10回の線分抽出を行う線分拡法による平面検出を行って成功した回数を示し、抽出の成否（垂直）（correct extraction(vertical))は、列方向のデータ列毎についての抽出の成否を示す。また、 No. 1— No. 5は、上述した Zig-Zag-Shape判別処理を取り入れていない従来の線分拡張法による平面検出処理の条件、 No. 6は、 Zig-Zag-Shape判別処理を行った本変形例における平面検出方法の条件を示す。図 55C及び図 55Dは、線分拡張法により平面検出した結果を示す図であって、それぞれ本変形例における手法により平面検出した結果、従来の線分拡張法により平面検出した結果 (比較例）を示す。図 55Bに示すように、従来の手法においては、線分抽出（Line Fitting)において推定のための閾値パラメータ max_dを大きくする（ max_d= 25, 30)と検出率が下がり、閾値 max_d小さくする (max_d= 10, 15)と検出率が向上する。これに対して、本発明のように、ジグザグ形検証処理を導入することにより、大きな閾値 max_d= 30を設定しても、優れた検出結果を示すことがわかる。

すなわち、閾値 max_dを大きくすると、ノイズの影響が少なくなるものの、線分抽出が難しくなり、閾値 max_dを小さくすると、ノイズの影響を受けて誤検出が多くなつてしまう o図 56Bに示す床面を撮影した画像から 3次元距離データを取得した場合を図 56B 及び図 56Cに示す。何れ左図は、行方向の画素列（距離データ列）から線分を抽出した例、右図は列方向の画素列 (距離データ列)から線分を抽出した例を示す。図 5 6Bに示すように、閾値 max_dを小さくすると、ノイズの影響が大きくなり、ノイズの影響が大きい遠方などにおいては特に、線分をうまく検出することができない。一方、図 5 6Cに示すように、従来の線分抽出に更にジグザグ形判別処理を加えた場合、閾値 max_dを大きくしても、更にノイズの影響が大きい遠方の領域であっても線分が検出されていることがわ力る。

これにより、上述した如ぐそれぞれ異なる階段を撮影した画像から 3次元距離データを取得して平面検出することができる。例えば、図 11及び図 12に示すように、何れの場合も全ての踏面を平面として検出できている。なお、図 12Bでは、床面の一部も他の平面として検出成功して、る。

本変形例によれば、線分拡張法による平面検出を行う際、始めは大きな閾値を設定して線分を分割し、次に Zig-Zag-Shape判別処理により、閾値を超えるデータ点を持たない直線であってもジグザグ形である場合には、ノイズではなぐ複数平面からなる直線であるとして線分を分割するようにしたので、ノイズを含む距離情報力も複数の平面を精度よく検出することが可能となる。

このように、小さい段差も精度よく検出することができるため、例えばロボット装置が移動可能な環境内の階段などを認識することができ、二足歩行ロボット装置であれば、この検出結果を利用して階段昇降動作が可能となる。

更に、複数の平面によって構成されている凸凹の床面を歩行可能な平面だと誤認識することがなくなり、ロボット装置の移動などが更に簡単になる。

なお、本発明は上述した変形例のみに限定されるものではなぐ本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。また、上述した平面検出処理、階段認識処理、階段昇降制御処理のうち 1以上の任意の処理は、ハードウエアで構成しても、演算器 (CPU)にコンピュータプログラムを実行させることで実現してもよい。コンピュータプログラムとする場合には、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。

Claims

請求の範囲

[1] 1.移動手段により移動可能なロボット装置において、

3次元の距離データ力環境内に含まれる 1又は複数の平面を検出し、平面情報として出力する平面検出手段と、

上記平面情報力移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有する

ことを特徴とするロボット装置。

[2] 2.上記 3次元の距離データを取得する距離計測手段を有する

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[3] 3.上記移動手段として脚部を備え、この脚部により移動可能である

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[4] 4.上記階段認識手段は、

与えられた平面情報力移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出手段と、

上記階段検出手段から出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合手段とを有する

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[5] 5.上記階段検出手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、上記階段統合手段は、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である 2以上の踏面力なる踏面群を検出した場合、当該踏面群を何れをも含む一の踏面となるよう統合することを特徴とする請求の範囲第 4項記載のロボット装置。

[6] 6.上記階段認識手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し上記踏面情報とする

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[7] 7.上記踏面情報は、少なくとも移動方向に対して該踏面の手前側の境界を示すフロントエッジ及び奥側の境界を示すバックエッジの情報を含む

ことを特徴とする請求の範囲第 6項記載のロボット装置。

[8] 8.上記踏面情報は、上記フロントエッジ及びバックエッジに挟まれた領域である安全領域の左右両側に隣接した領域であって移動可能である確率が高いと推定されるマ一ジン領域を示す右側マージン情報及び左側マージン情報を有する

ことを特徴とする請求の範囲第 7項記載のロボット装置。

[9] 9.上記踏面情報は、上記平面情報に基づき踏面と推定された領域の重心を示す参照点情報を有する

ことを特徴とする請求の範囲第 7項記載のロボット装置。

[10] 10.上記参照点情報は、上記フロントエッジ及びバックエッジに挟まれた領域を示す安全領域の重心、該安全領域及びその両側に隣接した領域であって移動可能である確率が高、と推定されるマージン領域力なる踏面領域の重心、又は踏面となる平面を構成する点群力求まる中心点の何れかからなる参照点の位置情報を有することを特徴とする請求の範囲第 9項記載のロボット装置。

[11] 11.上記踏面情報は、踏面となる平面を構成する点群の 3次元座標情報を有することを特徴とする請求の範囲第 7項記載のロボット装置。

[12] 12.上記階段認識手段は、上記平面情報に基づき平面の境界を抽出して多角形を算出し、該多角形に基づき上記踏面情報を算出する

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[13] 13.上記多角形は、上記平面情報に基づき抽出された平面の境界に外接する凸多角形領域である

ことを特徴とする請求の範囲第 12項記載のロボット装置。

[14] 14.上記多角形は、上記平面情報に基づき抽出された平面の境界に内接する凸多角形領域である

ことを特徴とする請求の範囲第 12項記載のロボット装置。

[15] 15.上記多角形は、上記平面情報に基づき抽出された平面の境界を平滑化した非凸多角形領域であることを特徴とする請求の範囲第 12項記載のロボット装置。

[16] 16.上記平面情報は、一の平面毎に、法線ベクトル、境界を示す境界情報、重心位置を示す重心情報、大きさを示す面積情報、及び平面度から選択される 1以上の情報を有する

ことを特徴とする請求の範囲第 1項記載のロボット装置。

[17] 17.上記階段昇降制御手段は、現在移動中の移動面におけるノックエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 3項記載のロボット装置。

[18] 18.上記階段昇降制御手段は、現在移動中の移動面におけるノックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 17項記載のロボット装置。

[19] 19.上記階段昇降制御手段は、次に移動対象となる踏面を検出し、当該移動対象となる踏面に対畤した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 3項記載のロボット装置。

[20] 20.上記階段昇降制御手段は、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報から当該移動対象となる次段の踏面を検索する

ことを特徴とする請求の範囲第 19項記載のロボット装置。

[21] 21.上記階段昇降制御手段は、現在の移動面におけるノックエッジに対畤した所定位置に移動した後、次の移動対象となる踏面を検出し、当該踏面におけるフロントェッジに対畤した所定位置に移動し、当該踏面に移動する昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 3項記載のロボット装置。

[22] 22.上記階段昇降制御手段は、踏面に対する上記移動手段の位置を規定したパラメータを使用して昇降動作を制御することを特徴とする請求の範囲第 3項記載のロボット装置。

[23] 23.上記パラメータは、上記脚部の足上げ高さ又は足下げ高さに基づき決定されることを特徴とする請求の範囲第 22項記載のロボット装置。

[24] 24.階段を登る動作と降りる動作とで上記パラメータの数値を変更するパラメータ切り替え手段を有する

ことを特徴とする請求の範囲第 22項記載のロボット装置。

[25] 25.上記平面検出手段は、 3次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、

上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することを特徴とする請求の範囲第 1項記載のロボット装置。

[26] 26.上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にある力否力を再度推定することを特徴とする請求の範囲第 25項記載のロボット装置。

[27] 27.上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがある力否かを判別し、該偏りがある場合には該着目点にて該距離データ点群を分割する

ことを特徴とする請求の範囲第 25項記載のロボット装置。

[28] 28.上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第 1の線分を抽出し、該距離データ点群のうち該第 1の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群力第 2の線分を抽出し、該第 2の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することを特徴とする請求の範囲第 25項記載のロボット装置。

[29] 29.上記平面領域拡張手段は、同一の平面に属すると推定される 1以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新すると共に該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する

ことを特徴とする請求の範囲第 25項記載のロボット装置。

[30] 30.上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除!ヽた距離データ点群力再度平面を算出する平面再算出手段を更に有する

ことを特徴とする請求の範囲第 29項記載のロボット装置。

[31] 31.上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属する力否かを推定する

ことを特徴とする請求の範囲第 25項記載のロボット装置。

[32] 32.移動手段により移動可能なロボット装置の動作制御方法において、

3次元の距離データ力環境内に含まれる 1又は複数の平面を検出し、平面情報として出力する平面検出工程と、

上記平面情報力移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程と

を有することを特徴とするロボット装置の動作制御方法。

[33] 33.上記 3次元の距離データを取得する距離計測工程を有する

ことを特徴とする請求の範囲第 32項記載のロボット装置の動作制御方法。

[34] 34.上記移動手段は、機体脚部である

[35] 35.上記階段認識工程は、与えられた平面情報力移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出工程と、

上記階段検出工程にて出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合工程とを有する

[36] 36.上記階段検出工程では、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、上記階段統合工程では、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である 2以上の踏面からなる踏面群を検出した場合、当該踏面群を何れも含む一の踏面となるよう統合することを特徴とする請求の範囲第 35項記載のロボット装置の動作制御方法。

[37] 37.上記階段認識工程では、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し上記踏面情報とする

[38] 38.上記踏面情報は、少なくとも移動方向に対して該踏面の手前側の境界を示すフロントエッジ及び奥側の境界を示すバックエッジを示す情報を含む

ことを特徴とする請求の範囲第 37項記載のロボット装置の動作制御方法。

[39] 39.上記階段認識工程では、上記平面情報に基づき平面の境界を抽出して多角形を算出し、該多角形に基づき上記踏面情報を算出する

[40] 40.上記階段昇降制御工程では、現在移動中の移動面におけるバックエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 34項記載のロボット装置の動作制御方法。

[41] 41.上記階段昇降制御工程では、現在移動中の移動面におけるノックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対畤した所定位置に移動した後、昇降動作を実行するよう制御する

ことを特徴とする請求の範囲第 40項記載のロボット装置の動作制御方法。

[42] 42.上記階段昇降制御工程では、次に移動対象となる踏面を検出し、当該踏面に対畤した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御することを特徴とする請求の範囲第 34項記載のロボット装置の動作制御方法。

[43] 43.上記階段昇降制御工程では、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報から当該移動対象となる次段の踏面を検索する

ことを特徴とする請求の範囲第 42項記載のロボット装置の動作制御方法。

[44] 44.上記階段昇降制御工程では、現在の移動面におけるバックエッジに対畤した所定位置に移動した後、次の移動対象となる踏面を検出し、当該踏面におけるフロントエッジに対畤した所定位置に移動し、当該踏面に移動する昇降動作を実行するよう制御する

[45] 45.上記階段昇降制御工程では、踏面に対する移動手段の位置を規定するためのパラメータを使用して昇降動作を制御する

[46] 46.階段を登る動作と降りる動作とで上記パラメータの数値を変更するパラメータ切り替え工程を有する

ことを特徴とする請求の範囲第 45項記載のロボット装置の動作制御方法。

[47] 47.移動手段により移動可能な移動装置において、

上記平面情報力移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段とを有する

ことを特徴とする移動装置。