(57)【特許請求の範囲】
【請求項１】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記階段認識手段は、
与えられた平面情報から移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出手段と、
上記階段検出手段から出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合手段とを有し、
上記階段検出手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、
上記階段統合手段は、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である２以上の踏面からなる踏面群を検出した場合、当該踏面群を何れをも含む一の踏面となるよう統合する
ロボット装置。
【請求項２】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記階段認識手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し上記踏面情報とし、
上記踏面情報は、少なくとも移動方向に対して該踏面の手前側の境界を示すフロントエッジ及び奥側の境界を示すバックエッジの情報を含み、
上記踏面情報は、上記フロントエッジ及びバックエッジに挟まれた領域である安全領域の左右両側に隣接した領域であって移動可能である確率が高いと推定されるマージン領域を示す右側マージン情報及び左側マージン情報を有する
ロボット装置。
【請求項３】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記階段認識手段は、上記平面情報に基づき平面の境界を抽出して多角形を算出し、該多角形に基づき上記踏面情報を算出し、
上記多角形は、上記平面情報に基づき抽出された平面の境界に外接又は内接する凸多角形領域あるいは平面の境界を平滑化した非凸多角形領域である
ロボット装置。
【請求項４】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記移動手段として脚部を備え、この脚部により移動可能であり、
上記階段昇降制御手段は、現在移動中の移動面におけるバックエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御し、
上記階段昇降制御手段は、現在移動中の移動面におけるバックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御する
ロボット装置。
【請求項５】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記移動手段として脚部を備え、この脚部により移動可能であり、
上記階段昇降制御手段は、次に移動対象となる踏面を検出し、当該移動対象となる踏面に対峙した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御し、
上記階段昇降制御手段は、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報から当該移動対象となる次段の踏面を検索する
ロボット装置。
【請求項６】
移動手段により移動可能なロボット装置において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有し、
上記平面検出手段は、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、
上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出する
ロボット装置。
【請求項７】
上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定する
請求項６記載のロボット装置。
【請求項８】
上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から線分を抽出し、該距離データ点群のうち該線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群における距離データ点の分布に偏りがあるか否かを判別し、該偏りがある場合には該着目点にて該距離データ点群を分割する
請求項６記載のロボット装置。
【請求項９】
上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割する
請求項６記載のロボット装置。
【請求項１０】
上記平面領域拡張手段は、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新すると共に該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力する
請求項６記載のロボット装置。
【請求項１１】
上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有する
請求項１０記載のロボット装置。
【請求項１２】
上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定する
請求項６記載のロボット装置。
【請求項１３】
移動手段により移動可能なロボット装置の動作制御方法において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出工程と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程とを有し、
上記階段認識工程は、
与えられた平面情報から移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出工程と、
上記階段検出工程にて出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合工程とを有し、
上記階段検出工程では、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、
上記階段統合工程では、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である２以上の踏面からなる踏面群を検出した場合、当該踏面群を何れも含む一の踏面となるよう統合する
ロボット装置の動作制御方法。
【請求項１４】
移動手段により移動可能なロボット装置の動作制御方法において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出工程と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程とを有し、
上記移動手段は、機体脚部であり、
上記階段昇降制御工程では、現在移動中の移動面におけるバックエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御し、
上記階段昇降制御工程では、現在移動中の移動面におけるバックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御する
ロボット装置の動作制御方法。
【請求項１５】
移動手段により移動可能なロボット装置の動作制御方法において、
３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出工程と、
上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、
上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程とを有し、
上記移動手段は、機体脚部であり、
上記階段昇降制御工程では、次に移動対象となる踏面を検出し、当該踏面に対峙した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御し、
上記階段昇降制御工程では、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報から当該移動対象となる次段の踏面を検索する
ロボット装置の動作制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば脚部などの移動手段を有し、複数段からなる階段の昇降動作等を可能とするロボット装置、移動装置、及びその階段昇降方法に関する。
本出願は、日本国において２００４年３月１７日に出願された日本特許出願番号２００４−０７７２１４を基礎として優先権を主張するものであり、これらの出願は参照することにより、本出願に援用される。
【背景技術】
【０００２】
従来、車輪等の移動手段を有する移動装置を含むロボット装置に対し、段差のある環境や、階段の昇降動作をさせることを可能とする技術が複数開示されている。
例えば、対象物体の形状特徴点を認識するため対象物体のノードデータを有した、図１に示す既知の階段情報を用いて昇降動作を行う方法（特許第３１７６７０１号公報）や、図２に示すように、多数の接触センサ６０２が保護膜６０３で被覆されつつマトリクス状に足平６０１の裏面の全面的に貼り付けられた足平を利用して、階段昇降動作を行う方法がある（特許第３２７８４６７号公報）。更に、図３に示すように、足底の脇に赤外センサを装備し、階段にランドマークテープを付与することで、階段昇降動作を行う方法がある（特許第３３３０７１０号公報）。これは、図３Ａに示すように、２足歩行可能なロボット装置の足平６２２Ｒ／Ｌの左右の脇部分に複数の光センサ検出部６８２を設け、黒色ペイントなどの光をよく吸収する塗料で描いた直線からなる所定幅の面領域であるランドマーカ６８０を利用することで、対になっているセンサ出力を比較することでランドマーカ６８０に対する相対的な方角を検出するものである。このような足平６２２Ｒ／Ｌとランドマーカ６８０とを使用することで、図３Ｂに示すように、階段６９０の位置を認識することが可能となる。
しかしながら、特許第３１７６７０１号公報に記載の技術においては、既知の階段情報に基づいているため、未知の環境においては適応することができなく、例えば自律型のロボット装置などに適用することが困難である。また、特許第３２７８４６７号公報及び特許第３３３０７１０号公報に記載の技術においては、足裏もしくは足底の脇に設けられた複数のセンサを利用して階段昇降動作を行うため、着床するまで階段の情報が取得できない。したがって、遠くからの観測が不可能である。このため、ある程度目の前に階段があることが予測されるときにしか利用することができない。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
本発明は、このような従来の実情に鑑みて提案されたものであり、移動体自身が階段に係る情報を取得して自律的に階段昇降動作を可能とするロボット装置及び移動装置、並びにロボット装置の動作制御方法を提供することを目的とする。
上述した目的を達成するために、本発明に係るロボット装置は、移動手段により移動可能なロボット装置において、３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有することを特徴とする。
本発明においては、移動手段として例えば脚部などを備えて移動可能なロボット装置において、階段の踏面の例えば大きさや位置などに関する踏面情報から、その足底がその踏面に載せることができる大きさか否かを判断したり、階段の段差を示す蹴り上げの情報からその高さの踏面への移動が可能か否かを判断し、移動可能であると判断した場合には自律的に位置決めすることで階段を登ったり降りたりすることが可能となる。
また、上記３次元の距離データを取得する距離計測手段を有することができ、階段を検出した際には自律的に昇降動作が可能となる。
また、上記階段認識手段は、与えられた平面情報から移動可能な平面を有する階段を検出して統合前階段情報を出力する階段検出手段と、上記階段検出手段から出力される時間的に異なる複数の統合前階段情報を統計的に処理することにより統合した統合済階段情報を上記階段情報として出力する階段統合手段とを有することができ、例えば視野が狭いロボット装置であったり、１度の検出処理ではうまく階段認識できないような場合であっても、時間的に統計的に統合した統合済階段情報とすることで、正確かつ高域な認識結果を得ることができる。
更に、上記階段検出手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し、この認識結果である踏面情報を上記統合前階段情報として出力し、上記階段統合手段は、時間的に前後する踏面情報から、所定の閾値より大きい重複領域を有しかつ相対的な高さの違いが所定の閾値以下である２以上の踏面からなる踏面群を検出した場合、当該踏面群を何れをも含む一の踏面となるよう統合することができ、統合の際には統合すべきとして選択された踏面を全て含むように統合することで、広い範囲にわたって認識結果を得ることができる。
更にまた、上記階段認識手段は、上記平面情報に基づき踏面の大きさ及び空間的な位置を認識し上記踏面情報とすることができ、踏面情報は、少なくとも移動方向に対して該踏面の手前側の境界を示すフロントエッジ及び奥側の境界を示すバックエッジの情報を含むものとすることができ、踏面のフロントエッジ及びバックエッジを認識するため、例えばスパイラル形状の階段などであっても正確に踏面を認識して階段昇降動作を可能にする。
また、上記フロントエッジ及びバックエッジに挟まれた領域である安全領域の左右両側に隣接した領域であって移動可能である確率が高いと推定されるマージン領域を示す右側マージン情報及び左側マージン情報、上記平面情報に基づき踏面と推定された領域の重心を示す参照点情報、踏面となる平面を構成する点群の３次元座標情報などを有することができる。これらの踏面情報を使用することで、階段昇降動作をより正確に制御することができる。
更に、上記階段認識手段は、上記平面情報に基づき平面の境界を抽出して多角形を算出し、該多角形に基づき上記踏面情報を算出することができ、例えば、視野が狭い場合、３次元距離データの信頼性が高いような場合には、上記多角形は、上記平面情報に基づき抽出された平面の境界に外接する凸多角形領域とすることができ、実際に検出されている平面を含んだ領域とすることができる。一方、ノイズが多い距離データなどの場合には、上記多角形は、上記平面情報に基づき抽出された平面の境界に内接する凸多角形領域とすることができ、実際に検出されている平面に内包される領域とすることで、ノイズ部分をカットして正確に踏面を検出することができる。
更にまた、現在移動中の移動面におけるバックエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御することができ、例えば蹴り上げが小さい階段など、フロントエッジとバックエッジが重なるような場合には、バックエッジを目標に移動して昇降動作することができる。同様に、フロントエッジを目標に移動して昇降動作を行ってもよい。
また、上記階段昇降制御手段は、現在移動中の移動面におけるバックエッジが確認できない場合は、次に昇降動作の対象となる次段の踏面におけるフロントエッジに対峙した所定位置に移動した後、昇降動作を実行するよう制御することができ、例えば床面を移動していて階段を検出した場合、床面のバックエッジが階段初段のフロントエッジに重ならない場合があり、そのような場合は階段の初段、すなわち昇降動作の対象となる次段の踏面のフロントエッジを目標に移動して昇降動作をすることができる。
また、上記階段昇降制御手段は、次に移動対象となる踏面を検出し、当該移動対象となる踏面に対峙した所定位置に移動する一連の動作を行って昇降動作を実行するよう制御することができ、新たな踏面に移動する毎に、踏面に対してサーチ・アライン・アプローチ動作を実行することで昇降動作を可能にする。
更に、上記階段昇降制御手段は、現在位置から次に移動対象となる次段又は次段以降の踏面が検出できない場合、過去に取得した階段情報から当該移動対象となる次段の踏面を検索することができ、予め数段上又は下の階段情報を取得しておくことで、ロボット装置が自身の直近の情報が得られないような構成で視野が狭い場合であっても昇降動作を可能とする。
また、上記階段昇降制御手段は、現在の移動面におけるバックエッジに対峙した所定位置に移動した後、次の移動対象となる踏面を検出し、当該踏面におけるフロントエッジに対峙した所定位置に移動し、当該踏面に移動する昇降動作を実行するよう制御することができ、フロントエッジ及びバックエッジを使用することで、両エッジが平行していない螺旋状の階段であっても昇降動作を可能とする。
更に、上記昇降制御手段は、踏面に対する上記移動手段の位置を規定したパラメータを使用して昇降動作を制御することができ、このパラメータは、例えば上記脚部の足上げ高さ又は足下げ高さに基づき決定されることができる。そして、階段を登る動作と降りる動作とで上記パラメータの数値を変更するパラメータ切り替え手段を有することができ、階段を登る動作であっても、降りる動作であってもパラメータ変更するのみで同様に制御することができる。
また、上記平面検出手段は、３次元空間で同一平面上にあると推定される距離データ点群毎に線分を抽出する線分抽出手段と、上記線分抽出手段によって抽出された線分群から同一平面に属すると推定される複数の線分を抽出し該複数の線分から平面を算出する平面領域拡張手段とを有し、
上記線分抽出手段は、距離データ点の分布に応じて適応的に線分を抽出することができ、線分抽出手段は、３次元の距離データが同一平面上にある場合同一直線上に並ぶことを利用して線分を抽出するが、この際、ノイズなどの影響により距離データ点の分布に違いが生じるため、この距離データの分布に応じて適応的に線分を抽出する（Adaptive Line Fitting）ことにより、ノイズに対してロバストに、精確な線分抽出を可能とし、抽出された多数の線分から線分拡張法により平面を求めるため、ノイズの影響などにより、本来複数平面が存在するのに１つの平面としたり、１つの平面しか存在しないのに複数平面としたりすることなく精確に平面抽出することができる。
更に、上記線分抽出手段は、上記距離データ点間の距離に基づき同一平面上にあると推定される距離データ点群を抽出し、該距離データ点群における距離データ点の分布に基づき、当該距離データ点群が同一平面上にあるか否かを再度推定することができ、距離データ点の３次元空間における距離に基づき一旦距離データ点群を抽出しておき、データ点の分布に基づき再度同一平面上にあるか否かを推定することにより精確に線分抽出することができる。
更にまた、上記線分抽出手段は、上記同一平面上にあると推定される距離データ点群から第１の線分を抽出し、該距離データ点群のうち該第１の線分との距離が最も大きい距離データ点を着目点とし、当該距離が所定の閾値以下である場合に該距離データ点群から第２の線分を抽出し、該第２の線分の一方側に距離データ点が所定の数以上連続して存在するか否かを判定し、所定の数以上連続して存在する場合に該距離データ点群を該着目点にて分割することができ、例えば抽出したデータ点群の端点を結ぶ線分を第１の線分とし、上記距離が大きい点が存在する場合には、例えば最小二乗法により第２の線分を生成し、この第２の線分において一方側に連続して複数のデータ点が存在する場合には、データ点群は例えば線分に対してジグザグな形などをとっていることが想定でき、したがって抽出したデータ点群には偏りがあると判断して、上記着目点などにてデータ点群を分割することができる。
また、上記平面領域拡張手段は、同一の平面に属すると推定される１以上の線分を選択して基準平面を算出し、該基準平面と同一平面に属すると推定される線分を該線分群から拡張用線分として検索し、該拡張用線分により該基準平面を更新すると共に該基準平面の領域を拡張する処理を繰り返し、更新が終了した平面を更新済平面として出力することができ、同一平面に属するとされる線分により平面領域拡張処理及び平面更新処理を行うことができる。
更に、上記更新済平面に属する距離データ点群において、当該更新済平面との距離が所定の閾値を超える距離データ点が存在する場合、これを除いた距離データ点群から再度平面を算出する平面再算出手段を更に有することができ、更新済平面はそれに属する全線分の平均した平面として得られているため、これから大きく外れた距離データ点を除いたデータ点群から再度平面を求めることで、よりノイズなどの影響を低減した検出結果を得ることができる。
更にまた、上記平面領域拡張手段は、線分により定まる平面と上記基準平面との誤差に基づき当該線分が該基準平面と同一平面に属するか否かを推定することができ、例えば平面方程式の２乗平均誤差などに基づきノイズの影響であるのか、異なる平面なのかを判別して更に正確に平面検出することができる。
本発明に係るロボット装置の動作制御方法は、移動手段により移動可能なロボット装置の動作制御方法において、３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出工程と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識工程と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御工程とを有することを特徴とする。
本発明に係る移動装置は、移動手段により移動可能な移動装置において、３次元の距離データから環境内に含まれる１又は複数の平面を検出し、平面情報として出力する平面検出手段と、上記平面情報から移動可能な平面を有する階段を認識し、該階段の踏面に関する踏面情報及び蹴り上げ情報を有する階段情報を出力する階段認識手段と、上記階段情報に基づき、階段昇降可能か否かを判断し、昇降動作が可能であると判断した場合には、その踏面に対して自律的に位置決めして階段昇降動作を制御する階段昇降制御手段を有することを特徴とする。
本発明によれば、移動手段として例えば脚部などを備えて移動可能なロボット装置及び移動装置において、階段の踏面の例えば大きさや位置などに関する踏面情報から、その足底がその踏面に載せることができる大きさか否かを判断したり、階段の段差を示す蹴り上げの情報からその高さの踏面への移動が可能か否かを判断し、移動可能であると判断した場合には自律的に位置決めすることで階段を登ったり降りたりすることが可能となる。
本発明の更に他の目的、本発明によって得られる利点は、以下において図面を参照して説明される実施に形態から一層明らかにされるであろう。
【図面の簡単な説明】
【０００４】
【図１】図１は、従来の昇降動作を説明する図である。
【図２】図２は、従来の昇降動作を説明する図である。
【図３】図３Ａ、図３Ｂは、従来の昇降動作を説明する図である。
【図４】図４は、本発明の実施の形態におけるロボット装置の概観を示す斜視図である。
【図５】図５は、ロボット装置が具備する関節自由度構成を模式的に示す図である。
【図６】図６は、ロボット装置の制御システム構成を示す模式図である。
【図７】図７は、ロボット装置がステレオデータから階段昇降動作を発現するまでの処理を実行するシステムを示す機能ブロック図である。
【図８】図８Ａは、ロボット装置が外界を撮影している様子を示す模式図、図８Ｂは、ロボット装置の足底の大きさを示す図である。
【図９】図９は、階段検出を説明する図であって、図９Ａは、階段を正面から見た図、図９Ｂは、階段を側面から見た図、図９Ｃは、階段を斜めから見た図である。
【図１０】図１０は、階段検出の他の例を示す説明する図であって、図１０Ａは、階段を正面から見た図、図１０Ｂは、階段を側面から見た図、図１０Ｃは、階段を斜めから見た図である。
【図１１】図１１は、図９の階段を検出した結果の一例を示す図であって、図１１Ａは、図９の階段を撮影した場合の画像を示す模式図、図１１Ｂ乃至図１１Ｄは、図１１Ａに示す画像から取得した３次元の距離データを示す図である。
【図１２】図１２は、図１０の階段を検出した結果の一例を示す図であって、図１２Ａは、図１０の階段を撮影した場合の画像を示す模式図、図１２Ｂ乃至図１２Ｄは、図１２Ａに示す画像から取得した３次元の距離データを示す図である。
【図１３】図１３Ａは、階段を撮影した画像を示す模式図、図１３Ｂは、図１３Ａから取得した３次元距離データから４つの平面領域Ａ、Ｂ、Ｃ、Ｄを検出した結果を示す図である。  階段を検出した結果の一例を示す図である。
【図１４】図１４は、階段認識器を示す機能ブロック図である。
【図１５】図１５は、階段検出処理の手順を示すフローチャートである。
【図１６】図１６Ａ、図１６Ｂは、多角形を示す模式図である。
【図１７】図１７は、Melkmanのアルゴリズムを説明するための模式図である。
【図１８】図１８Ａ、図１８Ｂは、Sklanskyのアルゴリズムにより多角形を求める方法を説明するための模式図である。
【図１９】図１９は、非凸多角形形状の階段について発生する問題を説明するための模式図であって、図１９Ａは、入力される平面を示す図、図１９Ｂは、凸包による非凸多角形形状の階段の多角形表現結果を示す図である。
【図２０】図２０は、平滑化によって入力平面を包含する多角形を求める方法を示す模式図であって、図２０Ａは、入力された平面を示す図、図２０Ｂは、入力平面を示す多角形から不連続なギャップを除去し平滑化した多角形を示す図、図２０Ｃは、図２０Ｂで得られた多角形に対してラインフィッティングにより更に平滑化した多角形を示す図である。
【図２１】図２１は、ギャップ除去とラインフィットによる平滑化によって入力平面を包含する多角形を求める処理のプログラム例を示す図である。
【図２２】図２２Ａ、図２２Ｂは、階段パラメータの算出方法を説明するための模式図である。
【図２３】図２３は、最終的に認識される踏面及び階段パラメータを説明するための模式図である。
【図２４】図２４Ａ、図２４Ｂは、階段を示す模式図である。
【図２５】図２５は、階段統合処理の方法を示すフローチャートである。
【図２６】図２６は、オーバーラップしている階段データを統合する処理を説明するための模式図である。
【図２７】図２７は、アライン動作を説明するための図である。
【図２８】図２８は、アプローチ動作を説明するための模式図である。
【図２９】図２９は、階段昇降動作の手順を示すフローチャートである。
【図３０】図３０は、サーチ・アライン・アプローチ処理方法を示すフローチャートである。
【図３１】図３１は、昇降動作処理の方法を示すフローチャートである。
【図３２】図３２は、ロボット装置が認識しているか又は認識する予定の階段面を示す模式図である。
【図３３】図３３は、昇降動作処理の方法を示すフローチャートである。
【図３４】図３４は、昇降動作処理の方法を示すフローチャートである。
【図３５】図３５Ａは、ロボット装置により認識されている踏面と足底の関係を説明するための図であり、図３５Ｂは、各部の寸法を示す図である。
【図３６】図３６は、ロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。
【図３７】図３７は、ロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。
【図３８】図３８は、単一の段部とロボット装置の足底の関係を示す図である。
【図３９】図３９は、単一の凹部とロボット装置の足底の関係を示す図である。
【図４０】図４０は、本変形例における平面検出装置を示す機能ブロック図である。
【図４１】図４１は、テクスチャを付与する手段を有しているロボット装置を説明するための図である。
【図４２】図４２は、本変形例における線分拡張法による平面検出方法を説明する図である。
【図４３】図４３は、線分拡張法による平面検出処理を示すフローチャートである。
【図４４】図４４は、本変形例における線分抽出部における処理の詳細を示すフローチャートである。
【図４５】図４５は、距離データ点の分布の様子を示す図であって、図４５Ａは、データの分布が線分に対してジグザグ形である場合、図４５Ｂは、ノイズなどにより線分近傍に一様に分布している場合を示す模式図である。
【図４６】図４６は、本変形例におけるZig-Zag-Shape判別方法を示すフローチャートである。
【図４７】図４７は、上記Zig-Zag-Shape判別処理のプログラム例を示す図である。
【図４８】図４８は、Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。
【図４９】図４９は、本変形例における領域拡張処理を説明するための模式図である。
【図５０】図５０は、本変形例における領域拡張部における領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。
【図５１】図５１は、端点と直線との距離が等しくても平面方程式の２乗平均誤差ｒｍｓが異なる例を示す図であって、図５１Ａは、ノイズなどの影響により線分が平面からずれている場合、図５１Ｂは、線分が属する他の平面が存在する場合を示す模式図である。
【図５２】図５２は、領域種の選択処理を示す図である。
【図５３】図５３は、領域拡張処理を示す図である。
【図５４】図５４Ａは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図５４Ｂは、縦軸をｘ、横軸をｙ、各データ点の濃淡でｚ軸を表現して３次元距離データ及び、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す図、図５４Ｃは、図５４Ｂに示す直線群から領域拡張処理により得られた平面領域を示す図である。
【図５５】図５５は、床面に段差を一段置いたときの本変形例における平面検出方法と従来の平面検出方法との結果の違いを説明するための図であって、図５５Ａは、観察された画像を示す模式図、図５５Ｂは、実験条件を示す図、図５５Ｃは、本変形例における平面検出方法により平面検出された結果を示す図、図５５Ｄは、従来の平面検出方法により平面検出された結果を示す図である。
【図５６】図５６Ａは、床面を撮影した画像を示す模式図、図５６Ｂ及び図５６Ｃは、図５６Ａに示す床面を撮影して取得した３次元距離データから水平方向及び垂直方向の距離データ点列から、それぞれ本変形例の線分検出により検出した線分及び従来の線分検出により検出した線分を示す図である。
【発明を実施するための最良の形態】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、周囲の環境に存在する階段などの段差を認識する段差認識装置を搭載した自律的に動作可能なロボット装置に適用したものである。
本実施の形態におけるロボット装置は、ステレオビジョンなどにより得られた距離情報（距離データ）から抽出した複数平面から階段を認識し、この階段認識結果を利用して階段昇降動作を可能とするものである。
（１）ロボット装置 
ここでは、まず、このようなロボット装置の一例として２足歩行タイプのロボット装置を例にとって説明する。このロボット装置は、住環境その他の日常生活上の様々な場面における人的活動を支援する実用ロボットであり、内部状態（怒り、悲しみ、喜び、楽しみ等）に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボット装置である。なお、ここでは、２足歩行型のロボット装置を例にとって説明するが、階段認識装置は、２足歩行のロボット装置に限らず、脚式移動型のロボット装置に搭載すればロボット装置に階段昇降動作を実行させることができる。
図４は、本実施の形態におけるロボット装置の概観を示す斜視図である。図４に示すように、ロボット装置２０１は、体幹部ユニット２０２の所定の位置に頭部ユニット２０３が連結されると共に、左右２つの腕部ユニット２０４Ｒ／Ｌと、左右２つの脚部ユニット２０５Ｒ／Ｌが連結されて構成されている（ただし、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下において同じ。）。
このロボット装置２０１が具備する関節自由度構成を図５に模式的に示す。頭部ユニット２０３を支持する首関節は、首関節ヨー軸１０１と、首関節ピッチ軸１０２と、首関節ロール軸１０３という３自由度を有している。
また、上肢を構成する各々の腕部ユニット２０４Ｒ／Ｌは、肩関節ピッチ軸１０７と、肩関節ロール軸１０８と、上腕ヨー軸１０９と、肘関節ピッチ軸１１０と、前腕ヨー軸１１１と、手首関節ピッチ軸１１２と、手首関節ロール輪１１３と、手部１１４とで構成される。手部１１４は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部１１４の動作は、ロボット装置２０１の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は７自由度を有するとする。
また、体幹部ユニット２０２は、体幹ピッチ軸１０４と、体幹ロール軸１０５と、体幹ヨー軸１０６という３自由度を有する。
また、下肢を構成する各々の脚部ユニット２０５Ｒ／Ｌは、股関節ヨー軸１１５と、股関節ピッチ軸１１６と、股関節ロール軸１１７と、膝関節ピッチ軸１１８と、足首関節ピッチ軸１１９と、足首関節ロール軸１２０と、足底１２１とで構成される。本明細書中では、股関節ピッチ軸１１６と股関節ロール軸１１７の交点は、ロボット装置２０１の股関節位置を定義する。人体の足底１２１は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット装置２０１の足底は、ゼロ自由度とする。したがって、各脚部は、６自由度で構成される。
以上を総括すれば、ロボット装置２０１全体としては、合計で３＋７×２＋３＋６×２＝３２自由度を有することになる。ただし、エンターテインメント向けのロボット装置２０１が必ずしも３２自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
上述したようなロボット装置２０１がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行うこと等の要請から、アクチュエータは小型かつ軽量であることが好ましい。
このようなロボット装置は、ロボット装置全体の動作を制御する制御システムを例えば体幹部ユニット２０２等に備える。図６は、ロボット装置２０１の制御システム構成を示す模式図である。図６に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール２００と、アクチュエータ３５０の駆動等、ロボット装置２０１の全身協調運動を制御する運動制御モジュール３００とで構成される。
思考制御モジュール２００は、情緒判断や感情表現に関する演算処理を実行するＣＰＵ（Central Processing Unit）２１１や、ＲＡＭ（Random Access Memory）２１２、ＲＯＭ（Read Only Memory）２１３及び外部記憶装置（ハード・ディスク・ドライブ等）２１４等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。
この思考制御モジュール２００は、画像入力装置２５１から入力される画像データや音声入力装置２５２から入力される音声データ等、外界からの刺激等に従って、ロボット装置２０１の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット装置２０１の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。ここで、画像入力装置２５１は、例えばＣＣＤ（Charge Coupled Device）カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。また、音声入力装置２５２は、例えばマイクロホンを複数備えている。
思考制御モジュール２００は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール３００に対して指令を発行する。
一方の運動制御モジュール３００は、ロボット装置２０１の全身協調運動を制御するＣＰＵ３１１や、ＲＡＭ３１２、ＲＯＭ３１３及び外部記憶装置（ハード・ディスク・ドライブ等）３１４等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置３１４には、例えば、オフラインで算出された歩行パターンや目標とするＺＭＰ軌道、その他の行動計画を蓄積することができる。
この運動制御モジュール３００には、図５に示したロボット装置２０１の全身に分散するそれぞれの関節自由度を実現するアクチュエータ３５０、対象物との距離を測定する距離計測センサ（図示せず）、体幹部ユニット２０２の姿勢や傾斜を計測する姿勢センサ３５１、左右の足底の離床又は着床を検出する接地確認センサ３５２，３５３、足底１２１の足底１２１に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置３５４等の各種の装置が、バス・インターフェース（Ｉ／Ｆ）３１０経由で接続されている。ここで、姿勢センサ３５１は、例えば加速度センサとジャイロ・センサの組み合わせによって構成され、接地確認センサ３５２，３５３は、近接センサ又はマイクロ・スイッチ等で構成される。
思考制御モジュール２００と運動制御モジュール３００は、共通のプラットフォーム上で構築され、両者間はバス・インターフェース２１０，３１０を介して相互接続されている。
運動制御モジュール３００では、思考制御モジュール２００から指示された行動を体現すべく、各アクチュエータ３５０による全身協調運動を制御する。すなわち、ＣＰＵ３１１は、思考制御モジュール２００から指示された行動に応じた動作パターンを外部記憶装置３１４から取り出し、又は、内部的に動作パターンを生成する。そして、ＣＰＵ３１１は、指定された動作パターンに従って、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示する指令値を各アクチュエータ３５０に転送する。
また、ＣＰＵ３１１は、姿勢センサ３５１の出力信号によりロボット装置２０１の体幹部ユニット２０２の姿勢や傾きを検出するとともに、各接地確認センサ３５２，３５３の出力信号により各脚部ユニット２０５Ｒ／Ｌが遊脚又は立脚の何れの状態であるかを検出することによって、ロボット装置２０１の全身協調運動を適応的に制御することができる。更に、ＣＰＵ３１１は、ＺＭＰ位置が常にＺＭＰ安定領域の中心に向かうように、ロボット装置２０１の姿勢や動作を制御する。
また、運動制御モジュール３００は、思考制御モジュール２００において決定された意思通りの行動がどの程度発現されたか、すなわち処理の状況を、思考制御モジュール２００に返すようになっている。このようにしてロボット装置２０１は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
（２）ロボット装置の動作制御方法 
上述のロボット装置においては、頭部ユニット２０３にステレオビジョンシステムを搭載し、外界の３次元距離情報を取得することができる。次に、このようなロボット装置などに好適に搭載されるものであって、ロボット装置が、ステレオビジョンシステムにより周囲の環境から獲得した３次元距離データを使用して平面を検出し、この平面検出結果に基づき階段を認識し、この階段認識結果を使用して階段昇降動作を行う一連の処理について説明する。
図７は、ロボット装置がステレオデータから階段昇降動作を発現するまでの処理を実行するシステムを示す機能ブロック図である。図７に示すように、ロボット装置は、３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）１と、ステレオビジョンシステム１からステレオデータＤ１が入力され、このステレオデータＤ１から環境内の平面を検出する平面検出器（Plane Segmentation/Extractor）２と、平面検出器２から出力される平面データＤ２から階段を認識する階段認識器（Stair Recognition）３と、階段認識部２により認識された認識結果である階段データＤ４を使用して階段昇降動作をするための動作制御指令Ｄ５を出力する階段昇降制御器（Stair Climber）４とを備える。
そして、ロボット装置は、先ず、ステレオビジョンによって外界を観測し、両眼の視差によって算出される３次元距離情報であるステレオデータＤ１を画像として出力する。すなわち、人間の両眼に相当する左右２つのカメラからの画像入力を各画素近傍毎に比較し、その視差から対象までの距離を推定し、３次元距離情報を画像として出力（距離画像）する。この距離画像から平面検出器２によって平面を検出することで、環境内に存在する複数の平面を認識することができる。更に、階段認識器３によって、これら平面からロボット装置が昇降可能な平面を抽出し、その平面から階段を認識し階段データＤ４を出力する。そして階段昇降制御器４が階段データＤ４を用いて階段の昇降動作を実現する動作制御指令Ｄ５を出力する。
図８Ａは、ロボット装置２０１が外界を撮影している様子を示す模式図である。床面をｘ−ｙ平面とし、高さ方向をｚ方向としたとき、図８Ａに示すように、画像入力部（ステレオカメラ）を頭部ユニット２０３に有するロボット装置２０１の視野範囲は、ロボット装置２０１の前方の所定範囲となる。
ロボット装置２０１は、上述したＣＰＵ２１１において、画像入力装置２５１からのカラー画像及び視差画像と、各アクチュエータ３５０の全ての関節角度等のセンサデータとなどが入力されて各種の処理を実行するソフトウェア構成を実現する。
本実施の形態のロボット装置２０１におけるソフトウェアは、オブジェクト単位で構成され、ロボット装置の位置、移動量、周囲の障害物、及び環境地図等を認識し、ロボット装置が最終的に取るべき行動についての行動列を出力する各種認識処理等を行うことができる。なお、ロボット装置の位置を示す座標として、例えば、ランドマーク等の特定の物体等に基づく所定位置を座標の原点としたワールド基準系のカメラ座標系（以下、絶対座標ともいう。）と、ロボット装置自身を中心（座標の原点）としたロボット中心座標系（以下、相対座標ともいう。）との２つの座標を使用する。
ステレオビジョンシステム１では、カラー画像及びステレオカメラによる視差画像などの画像データが撮像された時間において、センサデータから割り出した関節角を使用してロボット装置２０１が中心に固定されたロボット中心座標系を頭部ユニット２０３に設けられた画像入力装置２５１の座標系へ変換する。この場合、本実施の形態においては、ロボット中心座標系からカメラ座標系の同次変換行列等を導出し、この同次変換行列とこれに対応する３次元距離データからなる距離画像を出力する。
本実施の形態におけるロボット装置は、自身の視野内に含まれる階段を認識することができ、その認識結果（以下、階段データという。）を使用して階段昇降動作を可能とする。したがって、階段昇降動作のためには、ロボット装置は、階段の大きさが自身の足底の大きさより小さいか、階段の高さが登ることができる又は降りることができる高さか否かなどの、階段の大きさについて様々な判断を行う必要がある。
ここで、本実施の形態においては、ロボット装置の足底の大きさを図８Ｂとした場合について説明する。すなわち、図８Ｂに示すように、ロボット装置２０１の前進方向をｘ軸方向、床面に平行でｘ方向と直交する方法をｙ方向としたとし、ロボット装置２０１が直立した際の両足のｙ方向の幅をfeet base width足底の大きさであて足首（脚部と足底の接続部）から前側の部分を足底前幅foot_front_size、足首から後ろ側の部分を足底の足底後ろ幅foot_back_sizeとするものとする。
ロボット装置２０１が環境内から検出する階段としては、例えば図９、図１０に示すようなものがある。図９Ａ、図１０Ａは、階段を正面から見た図、図９Ｂ、図１０Ｂは、階段を側面から見た図、図９Ｃ、図１０Ｃは、階段を斜めから見た図である。
ここで、人間、ロボット装置などが階段を昇降するために使用する面（足又は可動脚部を載せる面）を踏面といい、一の踏面からその次の踏面までの高さ（階段１段の階段の高さ）を蹴り上げという。また、以下では、階段は、地面に近い方から登るに従って１段目、２段目とカウントすることとする。
図９に示す階段ＳＴ１は、段数が３段の階段であり、蹴り上げ４ｃｍ、１，２段面の踏面の大きさは幅３０ｃｍ、奥行き１０ｃｍ、最上段である３段目の踏面のみ、幅３０ｃｍ、奥行き２１ｃｍとなっている。また、図１０に示す階段ＳＴ２も、段数が３段の階段であり、蹴り上げ３ｃｍ、１，２段面の踏面の大きさは幅３３ｃｍ、奥行き１２ｃｍ、最上段である３段目の踏面のみ、幅３３ｃｍ、奥行き３２ｃｍとなっている。ロボット装置がこれらの階段を認識した結果は後述する。
平面検出器２は、ステレオビジョン等の距離計測器から出力される距離情報（ステレオデータＤ１）から環境内に存在する複数の平面を検出し、平面データＤ２を出力する。平面の検出方法としては後述する線分拡張法の他、ハフ変換を利用した公知の平面検出技術を適用することができる。ただし、ノイズを含む距離データから階段のように複数平面を検出するには、後述する線分拡張法などによる平面検出を行うと正確に平面を検出することができる。
図１１、図１２は、階段を検出した結果の一例を示す図である。図１１及び図１２は、後述する平面検出方法により、それぞれ図９及び図１０に示す階段を撮影した画像から３次元距離データを取得して平面検出した例である。すなわち、図１１Ａは、図９の階段を撮影した場合の画像を示す模式図、図１１Ｂ乃至図１１Ｄは、図１１Ａに示す画像から取得した３次元の距離データを示す図である。また、図１２Ａは、図１０の階段を撮影した場合の画像を示す模式図、図１２Ｂ乃至図１２Ｄは、図１２Ａに示す画像から取得した３次元の距離データを示す図である。図１１及び図１２に示すように、何れの場合も全ての踏面を平面として検出できている。図１１Ｂは、下から１段目、２段目、３段目の踏面が平面検出されている例を示す。また、図１２Ｂは、床面の一部も他の平面として検出成功していることを示す。
すなわち、図１３Ａに示すように、例えば階段ＳＴ２を撮影した距離画像から平面検出すると、図１３Ｂに示すように、領域Ａ〜Ｄがそれぞれ、床面、１段目、２段目、３段目の踏面を示す平面として検出される。各領域Ａ〜Ｄに含まれる同一領域に示す点群は、それぞれ同一平面を構成すると推定された距離データ点群を示している。
階段認識器３には、こうして平面検出器２が検出した平面データＤ２が入力され、階段の形状、すなわち踏面の大きさ、階段の高さ（蹴り上げの大きさ）などを認識する。ここで、本実施の形態における階段認識器３は、詳細は後述するが、ロボット装置２０１が認識した踏面に含まれる領域（多角形）の手前側（ロボット装置に距離的に近い側）の境界（Front Edge）（以下、フロントエッジＦＥという。）と、踏面の奥側（ロボット装置に距離的に遠い側）の境界（Back Edge）（以下、バックエッジＢＥという。）とを階段データとして認識する。そして、階段昇降制御器４が階段データを利用して階段昇降動作を制御する。
次に、ロボット装置の階段昇降制御方法について具体的に説明する。なお、以下では、第１に、ロボット装置の階段認識方法、第２に認識した階段を利用して行う階段昇降動作、最後に平面検出方法の具体例として線分拡張法による平面検出方法の順にて説明する。
図１４は、図７に示す階段認識器を示す機能ブロック図である。図１４に示すように、階段認識器３は、平面検出器２から出力された平面データＤ２から階段を検出する階段検出器（Stair Extraction）５と、この階段検出器５が検出した階段データＤ３の時系列データ、すなわち異なる時間に検出された複数の階段データＤ３を統合することで更に正確に階段を認識する処理を行う階段統合器（Stair Merging）６とを有し、階段統合器にて統合した階段データＤ４が階段認識器３の出力となる。
階段検出器５は、平面検出器２から入力される平面データＤ２から階段を検出するが、
平面検出器２から入力される平面データＤ２は、１つの平面につき以下に示す複数の情報を有し、ステレオビジョンシステム１によって取り込まれた画像から検出された複数の平面毎の平面データが入力される。
すなわち、平面データＤ２は、平面毎に
１−１：平面を構成する点の数(number of supporting point) 
１−２：平面の中心となる点
１−３：平面パラメータ（法線ベクトル、原点からの距離）
１−４：平面を構成する多角形の境界
から構成される情報を有する。
この平面データに基づき、ロボット装置は、自身が接地している床面や踏面などの接地面と略水平な平面を選択し、下記の情報（以下、階段パラメータという。）を算出する。すなわち、
２−１：フロントエッジＦＥ、バックエッジＢＥ
２−２：階段の高さ
である。
ロボット装置が認識するフロントエッジＦＥ、バックエッジＢＥとは、上述した如く、階段の踏面の境界（線）を示すものであって、ロボット装置が対峙した場合に、多角形においてロボット装置に近い側の境界（手前側の境界）をフロントエッジＦＥ、ロボット装置から距離が離れている側の境界（奥側の境界）をバックエッジＢＥとする。これは、後述するように、例えば平面を構成する点を全て含む最小の多角形を求め、その手前側又は奥側の境界とすることができる。フロントエッジＦＥ及びバックエッジＢＥの情報としては、これらの端点の情報などとすることができる。また、上記多角形から階段の幅Ｗ（width）、階段の長さ（length）などの情報を得ることができる。また、階段の高さ（蹴り上げ）は、与えられた平面データＤ２の平面の中心点を利用して、２の平面の中心点間の高さ差としたり、上記多角形を求めた際の重心点を利用して２の重心点の高さの差としたりすることができる。なお、蹴り上げは、前段のバックエッジＢＥと後段のフロントエッジＦＥとの高さの差としたりしてもよい。
また、本実施の形態においては、フロントエッジＦＥ及びバックエッジＢＥに加えて更にフロントエッジＦＥ及びバックエッジＢＥに挟まれた領域（安全領域）の左右に隣接する領域であって移動可能である確率が高いと推定される領域をマージン（領域）として認識する。これらの求め方については後述する。このマージンを求めることにより、移動可能であると推定する踏面の領域を広く認識することができる。更に、踏面を構成するデータ点群の数や、上述の重心点などを１つ規定した参照点の情報などの情報（階段パラメータ）のセットを階段データＤ３とすることができる。
以上の階段データから下記の条件を満たす平面（階段）を抽出する。
３−１：フロントエッジＦＥ及びバックエッジＢＥの長さが所定の閾値以上
３−２：階段の高さ（height）が所定の閾値以下  
また、その他、
３−３：階段の幅Ｗ（width）が所定の閾値以上
３−４：階段の長さＬ（length）が所定の閾値以上  
など同時に満たすものを抽出することが好ましい。
図１５は、階段検出器５の階段検出処理の手順を示すフローチャートである。図１５に示すように、先ず、入力された平面データの平面パラメータなどに基づき、入力された平面が例えば接地面と水平であるか否かなど、歩行又は移動可能である平面か否かを判断する（ステップＳ１）。ここで、どのような平面が水平であるか又は移動可能であるかの条件は、ロボット装置の機能に応じて設定すればよい。例えば、入力平面の平面ベクトルをｎ（ｎｘ，ｎｙ，ｎｚ）とした場合、｜ｓｉｎ−１ｎｚ｜＞ｍｉｎｔｈであれば水平であると判断することができる。ここで、ｍｉｎｔｈは、水平面を判断するための閾値であり、例えば、使用する距離データ、平面検出など精度を考慮してｍｉｎｔｈ＝８０°などとして、水平面に対して±１０°までの傾きであれば水平と判断して検出するなどとすることができる。または、例えば、±３０°程度の傾きがあっても歩行または移動可能であれば、それらの角度範囲の平面を抽出するようにすればよい。ステップＳ１にて水平でないと判断した場合（ステップＳ１：Ｎｏ）、検出に失敗したことを出力して処理を終了し、次の平面データについての処理を実行する。
次に、平面が水平である場合（ステップＳ１：Ｙｅｓ）、平面の境界（形状）を認識するための処理を行う。ここでは、例えばSklanskyのアルゴリズム（J. Sklansky, ”Measuring concavity on a rectangular mosaic”, IEEE Trans Comput.21,1974,pp.1355-1364）やMelkmanのアルゴリズム（Melkman A., “On-line Construction of the Convex Hull of a Simple Polygon” Information Processing Letters 25,1987,p.11）などの凸包や、ノイズ除去による平滑化によって、入力平面を包含する多角形を求める（ステップＳ２）。そして、この多多角形の前後の境界線をフロントエッジ及びバックエッジなどの階段パラメータを求める（ステップＳ３）。そして、フロントエッジ及びバックエッジの両境界線から、本実施の形態においては、階段踏面を示す平面における幅Ｗ（width）及び長さＬ（length）を求め、これらの値が所定の閾値より大きいかどうか判断する（ステップＳ４）。踏面の幅及び長さが所定の閾値以上でない場合（ステップＳ４：Ｎｏ）、ロボット装置が移動可能な平面ではないとし、次の平面データについて再びステップＳ１からの処理を繰り返す。
平面の幅及び長さが所定の閾値以上である場合（ステップＳ４：Ｎｏ）、移動可能な踏面であると判断し左右のマージン（Left Margin, Right Margin）を計算し（ステップＳ５）、これらの情報を階段データＤ３として出力する。
次に、ステップＳ２において凸包によって入力平面を包含する多角形を求める方法について説明する。図１６は、凸多角形を示す模式図であって、図１６Ａは、入力された一の平面に属すると判断されたサポーティングポイント全て（同一平面であって連続した領域に含まれるとされる距離データ点群）が含まれる領域を示し、図１６Ｂは、図１６Ａに示す図形から求めた凸多角形である。ここで示す凸多角形は、与えられた平面図形（サポーティングポイントが含まれる領域）を含む最小の凸集合を求める凸包（convex hull）を利用したものとすることができる。なお、Ｇで示す点は、後述するが、踏面の幅Ｗを求める際に使用するもので、例えばサポーティングポイントが含まれる領域の重心などの点（参照点）を示す。
このような凸包を利用して凸多角形を求めるアルゴリズムの例として、上述したように、Melkmanのアルゴリズム、Sklanskyのアルゴリズムなどがある。図１７は、Melkmanのアルゴリズムを説明するための模式図である。図１７に示すように、与えられた図形に含まれる点を３点Ｐ１，Ｐ２，Ｐ３抽出し、点Ｐ１，Ｐ２を結ぶ線分を引き、点Ｐ１，Ｐ３、，Ｐ２，Ｐ３を通る直線を引く。これにより、３点Ｐ１，Ｐ２，Ｐ３からなる三角形ＡＲ４を含む５つの領域ＡＲ１〜ＡＲ５に区画される。そして、次に選択した点Ｐ４がどの領域に含まれるかを判断して多角形を形成しなおすという処理を繰り返して凸多角形を更新していく。例えばＰ４が、領域ＡＲ１に存在する場合、Ｐ１，Ｐ２，Ｐ４，Ｐ３の順序で結んだ線分に囲まれた領域が更新された凸多角形となる。また、領域ＡＲ３，ＡＲ４に点Ｐ４が存在する場合には、それぞれＰ１，Ｐ２，Ｐ３，Ｐ４の順序で結んだ線分に囲まれた領域，Ｐ１，Ｐ４，Ｐ２，Ｐ３の順序で結んだ線分に囲まれた領域として凸多角形を更新する。一方、領域ＡＲ４、すなわち凸多角形内部に点Ｐが含まれる場合は、凸多角形は更新せず、また、領域ＡＲ２に点Ｐ４がある場合は、点Ｐ３は除きＰ１，Ｐ２，Ｐ３の順序で結んだ線分に囲まれた領域として凸多角形を更新する。本実施の形態においては、全てのサポーティングポイントに対して、各点に含まれる領域を考慮して凸多角形を生成することができる。
図１８は、Sklanskyのアルゴリズムにより多角形を求める方法を説明するための模式図である。Sklanskyのアルゴリズムにより抽出される多角形は、Weakly Externally Visible Polygonと呼ばれるものであるが、上述のSklanskyのアル比して計算量が少なく、したがって高速演算が可能である。
与えられた図形を包含する凸多角形を求める場合、図１８Ａに示すように、与えられた図形１３１の境界上の任意の点ｘから図形１３１を含む円１３２に対して半直線を引く。このとき、点ｘから円１３２に引いた半直線のうち、図形１３１を横切らない半直線を引くことができた場合、この点は凸多角形の境界を構成する点であるとする。一方、図１８Ｂに示すように、与えられた図形１３３の境界上任意の他の点ｙから図形１３３を含む円１３４に半直線を引いた場合には、図形１３３を横切らない半直線を引くことができない。この場合は、この他の点ｙは凸多角形の境界を構成しないものとする。以上のようにして、順次各点が凸多角形の境界を構成するか否かを判断して選択された点のみからの図形を求めると、図１６Ａに示すような図形が得られる。
この図形を包含する凸多角形を求めることで、図１６Ｂの凸多角形を得ることができる。ここで、本実施の形態においては、ステレオビジョンシステム１の精度、特性などを考慮し、図１６Ａから凸多角形を求める場合には、図１６Ｂに示すように、図１６Ａの図形に外接する凸多角形を求めるものとして説明するが、カメラの精度、特性などを考慮し、例えば図１６Ａの図形に内接する凸多角形を求めるようにしてもよいことは勿論である。また、これらの方法を平面の傾き度合いや、周囲の状況に応じて使い分けるようにしてもよい。
また、図１５のステップＳ２において凸包によって入力平面を包含する多角形を求めた場合、非凸多角形形状の階段について問題が発生する。図１９は、この問題を示す模式図であって、図１９Ａは、入力される平面でありstep０は非凸多角形形状の階段である。図１９Ｂは、凸包によるstep0の多角形表現結果であり、非凸部分に関して望ましい結果と大きな乖離が生じている。このような非凸多角形を扱う方法として、ギャップ除去とラインフィットによる平滑化によって多角形を求める方法が考えられる。
ギャップ除去とラインフィットによる平滑化によって入力平面を包含する多角形を求める方法について説明する。図２０は、平滑化を示す模式図であって、図２０Ａは、入力された一の平面に属すると判断されたサポーティングポイント全て（同一平面であって連続した領域に含まれるとされる距離データ点群）が含まれる領域である入力多角形（input polygon）を示し、図２０Ｂは、入力平面を示す多角形から不連続なギャップを除去し（close gaps）平滑化した多角形（ギャップ除去多角形：gaps closed polygon）を示し、図２０Ｃは、図２０Ｂで得られた多角形に対してラインフィッティングにより（fit line segments）更に平滑化した多角形（平滑化多角形：smoothed polygon）である。ここで、図２１は、ギャップ除去とラインフィットによる平滑化によって入力平面を包含する多角形を求める処理のプログラム例を示す図であり、多角形から不連続なギャップを除去するClose gaps処理、及び得られた多角形に対してラインフィッティングにより更に平滑化するFit line segments処理を示している。
まず、ギャップ除去の方法について説明する。多角形を示す頂点から連続する３つの頂点を選び、この中央の点が端点を結んだ直線から大きく離れている場合にこの中央の点を除去する。残った頂点について、この処理を除去する点が無くなるまで続ける。
次に、ラインフィットの方法について説明する。多角形を示す頂点から連続する３つの頂点を選び、最小二乗法によりこれらの３点を近似する直線とこの直線と３点の誤差を求める。求められた全ての近似直線と誤差について、誤差の小さい順に並べ、誤差がある閾値より小さい場合に、中央の点を除去し、端点の位置を近似直線によって再計算する。この処理を除去する点が無くなるまで続ける。
次に、図１５のステップＳ３における処理について説明する。先ず、得られた多角形（図１６Ｂ、図２０Ｃ）から上述した階段パラメータを算出する。図２２は、階段パラメータの算出方法を説明するための模式図である。図２２Ａに示すように、得られた多角形１４０が、点１４１〜点１４７により囲まれる領域であるものとする。ここで、ロボット装置２０１からみて多角形１４０の前側の境界を構成する線分がフロントエッジＦＥ、奥側の境界を構成する線分がバックエッジＢＥである。
階段踏面の幅Ｗは、フロントエッジＦＥの中心点ＣＦＥと参照点Ｇとを結ぶ線分の長さをｄ１、バックエッジＢＥの中心点ＣＢＥと参照点Ｇとを結ぶ線分の長さをｄ２としたとき、幅Ｗ＝ｄ１＋ｄ２とすることができる。
ここで、参照点Ｇは、踏面となる平面の略中心であればよく、例えば、全サポーティングポイントの中心点としたり、多角形１４０の重心としたり、フロントエッジＦＥ及びバックエッジＢＥの端点を結んだ図２２Ｂに示す安全領域１５２の重心としたりすることができる。
こうして得られたフロントエッジＦＥ、バックエッジＢＥ、参照点Ｇに基づき、階段の長さＬ及び幅Ｗを求めステップＳ４の処理を実行する。階段の長さＬは、フロントエッジＦＥ及びバックエッジＢＥの長さのうち例えば短い方としたり、以下に示す左右のマージンを含めたフロントエッジＦＥ及び左右のマージンを含めたバックエッジＢＥのうち長い方としたりすることができる。
次に、ステップＳ５におけるマージン算出方法について説明する。図２３は、最終的に認識される踏面及び階段パラメータを説明するための模式図である。本実施の形態においては、図２２Ｂ、図２３に示すように、安全領域１５２の左右端部にマージンＭ１，Ｍ２を設け、この左右のマージンＭ１，Ｍ２を含んだ領域１５１を最終的に踏面として認識するものとする。左右のマージンＭ１，Ｍ２は、フロントエッジＦＥ及びバックエッジＢＥにて規定される安全領域１５２の外側に多角形がはみ出している場合、先ず、それらの点を選択する。図２２Ａにおいて、例えば右マージンＭ２を求める場合であれば、点１４２，１４３，１４６である。安全領域１５２の右側に隣接した右マージンＭ２を求める際は、安全領域１５２から最も離れた点である点１４２を選択し、この点１４２からフロントエッジＦＥ、バックエッジＢＥに対して垂線を下ろす。そして、これら垂線及びフロントエッジＦＥ、バックエッジＢＥに囲まれる領域１５１を踏面として認識するものとする。なお、このマージンの求め方としては、点１４２を通り、フロントエッジＦＥ又はバックエッジＢＥと交差する線分を引くのみでもよい。
ここで、図２２Ｂに示すように、フロントエッジＦＥと同一直線における左マージンＭ１の長さをｌｍｆとし、バックエッジＢＥと同一直線における左マージンＭ１の長さをｌｂｍとする。同様に、フロントエッジＦＥ，バックエッジＢＥとそれぞれ同一直線における右マージンＭ２の長さをｒｆｍ，ｒｂｍとする。
このようにして多角形からフロントエッジＦＥ、バックエッジＢＥを求めて階段を認識することの効果について説明しておく。図２４Ａ及び図２４Ｂは、２種類の階段を示す模式図である。図２４Ａに示すのは、図９、図１０に示したような踏面が長方形の形状の階段であるが、図２４Ｂは、螺旋（spiral）状の階段である。この図２４Ｂのような螺旋階段の場合、フロントエッジＦＥに対してバックエッジＢＥは平行でない。したがって、例えば単に検出した平面から長方形領域を抽出してしまうようなアルゴリズムを適用できない場合がある。したがって、本実施の形態のように、検出した平面から多角形を求め、フロントエッジＦＥ及びバックエッジＢＥを求めることにより、このような螺旋階段であってもロボット装置が昇降動作することが可能となる。
次に、図１４に示す階段統合器６について説明する。階段統合器６は、階段検出器５によって検出された階段データ（階段パラメータ）Ｄ３を入力とし、それらの階段データＤ３を時間的に統合することで、より正確かつ高域な階段の情報を推定するものである。例えば、ロボット装置の視野が狭い場合などにおいて、一度に階段全体を認識できない場合がある。そのような場合、例えば、例えば前フレームなどの古い階段データと例えば現フレームなどの新しい階段データとの中で、空間的にオーバーラップしている階段の組を探し、オーバーラップしている階段を統合することにより、新しい仮想的な階段を定義する。この作業をオーバーラップする階段がなくなるまで続けることによって正確な階段を認識することが可能となる。
図２５は、階段統合器６における階段統合処理の方法を示すフローチャートである。まず、現在の階段データ（New Stairs）と古い階段データ（Old Stairs）とを入力とし（ステップＳ１１）、新しい階段データ及び古い階段データの全てを１つの集合（ujnion）とする（ステップＳ１２）。これら合わされた階段データ集合において、空間的にオーバーラップしている階段データを検索し（ステップＳ１３）、オーバーラップしている組がある場合には（ステップＳ１４：Ｙｅｓ）、それらの階段を統合して当該階段データ集合に登録する（ステップＳ１５）。そして、ステップＳ１３、１４の処理を空間的にオーバーラップする階段の組がなくなるまで続け（ステップＳ１４：Ｎｏ）、最終的に更新された階段データ集合を階段データＤ４として出力する。
図２６は、オーバーラップしている階段データを統合するステップＳ１３における処理を説明するための模式図である。図２６は、空間的にオーバーラップしている階段データＳＴ１１，ＳＴ１２を示す。空間的にオーバーラップしているかどうかの判断には、例えば２つの階段データＳＴ１１，ＳＴ１２の参照点Ｇにおける高さの差（距離）と、左右のマージンを含めた踏面の領域が重なっている面積の大きさを利用することができる。すなわち、２つの階段の重心Ｇ１１，Ｇ１２の高さの差が閾値（maxdz）以下であり、かつ、オーバーラップしている面積の大きさが閾値（minarea）以上ある場合には、これらの２つの階段データが示す踏面はオーバーラップしていると判断することができる。その場合には図２６の下図に示すように、階段データＳＴ１１，ＳＴ１２を統合して重心Ｇ１３の階段データＳＴ１３とする。
ここで、統合に際しては、階段データＳＴ１１，ＳＴ１２を含む外枠の領域をステップＳＴ１３とし、統合前の階段データＳＴ１１及び階段データＳＴ１２の左右のマージンを除いた安全領域を含む領域を統合後の新たな安全領域１６５とし、階段データＳＴ１３からこの安全領域１６５を除いた領域をマージンＭ１，Ｍ２とする。統合後の安全領域１６５により統合されたフロントエッジＦＥ及びバックエッジＢＥを求めることができる。
すなわち、統合後の階段データＳＴ１３におけるフロントエッジＦＥの両端点は、階段データＳＴ１１のフロントエッジＦＥと階段データＳＴ１２のフロントエッジＦＥの左右の端点を比較し、右端点１６３はより右側にある方、左端点はより左側にある方とされる。また、フロントエッジＦＥのラインの位置は、階段データＳＴ１１のフロントエッジＦＥと階段データＳＴ１２のフロントエッジＦＥを比較してよりロボット装置に近い方（手前側）のライン位置とされる。同様にバックエッジＢＥもより奥側にある方の位置が選択され、より左右に広がるようにその左右の端点１６１，１６２が選択される。
なお、統合方法はこれに限るものではない。本実施の形態においては、ロボット装置の視界などを考慮してフロントエッジＦＥ及びバックエッジＢＥにより決まる四角形の領域、統合データＳＴ１３が共に最も大きくなるように統合するものとしているが、例えば、十分視界が広い場合や、距離データの精度が十分高い場合においては、２つの階段データを単に合わせた領域を統合後の階段データとするなどしてもよい。また、統合後の参照点Ｇは、階段データＳＴ１１と階段データＳＴ１２に含まれるサポーティングポイントの数の比に応じて重み付き平均をとって求めることができる。
次に、図７に示す階段昇降制御器４について説明する。階段昇降制御器４は、階段検出器及び階段統合器６によって統合されて検出された階段データＤ４を用いて実際にロボット装置が階段の昇降動作を行うための制御を行う。この昇降制御には、階段を探す動作も含むものとする。
本実施の形態において実現した階段昇降動作は、次の５つのステートマシンとして構築することができる。
４−１：サーチ（Search）動作  
ロボット装置の頭部に搭載されたステレオビジョンシステムにて距離画像を取得するため、首を振って周囲を見廻し、環境の情報を集める。
４−２：アライン（Align）動作  
階段に対して正対し、かつ、決められた一定量の距離位置に移動する。図２７は、アライン動作を説明するための図である。図２７において、領域１７０がロボット装置の認識した階段踏面の１段目であるとする。アライン動作においては、踏面１７０のフロントエッジＦＥの中心点から直交する方向に所定距離ａｄ（align_distance）離れた目的とする位置（以下、アライン位置という。）に移動する。この場合、現在のロボット装置の位置が点１７１であって、目的とするアライン位置が点１７２である場合、両者の距離が所定の閾値max_d以上離れている場合及びフロントエッジＦＥに直行する方向とロボット装置の向く方向との角度差が所定の閾値max_a以上ある場合、ロボット装置は目的とするアライン位置１７２に移動を開始し、これらの条件を満たしたとき、アライン動作が完了したと判断するものとする。これらの閾値としては、例えばmax_d＝３（ｃｍ）、max_a＝２°とすることができる。
４−３：アプローチ（Approach）動作  
アプローチ動作では、ロボット装置が階段の直前まで移動する。図２８は、アプローチ動作を説明するための模式図である。図２８に示すように、階段と正対しAlign_distanceだけ離れた目的位置であるアライン位置１７２に移動してアライン動作を終了したロボット装置２０１は、階段の昇降を行うために、踏面１７０のフロントエッジＦＥの中心点ＣＦＥとロボット装置２０１とが正対し、かつその距離が所定の値ａｘ（approach_x）となる目的の位置（以下、アプローチ位置という。）に移動する。
４−４：クライム（Climb）動作  
階段認識によって得られた階段データを元に階段昇降動作をする。次の踏面（段）に移動した場合であって、次の段の踏面が観測されている場合には、更に昇る又は下る動作を続ける。この動作を次の段がなくなるまで続けて行うことにより階段昇降動作が実現される。
４−５：フィニッシュ（Finish）動作  
クライム動作にて階段を登る動作をした場合は、最上段にいることを確認し、最上段の中央へ移動する。クライム動作にて降りる動作をした場合、又は図９に示すような階段ＳＴ１である場合には例えば向きを変えるなどして降りる動作をした場合には、床面に降りたことを認識することで階段昇降動作は終了される。
以上の階段昇降処理の動作方法について更に詳細に説明する。図２９は、階段昇降動作の手順を示すフローチャートである。図２９に示すように、階段昇降制御の処理が開始されると、サーチ（Search）・アライン（Align）・アプローチ（Approach）動作によって、階段を検索し、検索した階段に対して対峙した所定位置に移動（アライン）し、階段の１段目に近づくアプローチ動作を実行する（ステップＳ２１）。このサーチ・アライン・アプローチ動作が成功した場合（ステップＳ２２：Ｙｅｓ）、後述する方法にて階段昇降を行い（ステップＳ２３）、成功したことを出力する。近づくことに失敗した場合（ステップＳ２２：Ｎｏ）、失敗したことを出力して処理を終了する。この場合は、もう一度、ステップＳ２１の処理から繰り返すなどする。
ここで、ステップＳ２１におけるサーチ・アライン・アプローチのシーケンスは通常ロボット装置が物体や目的地に到達するために用いる制御と同じである。具体的には例えば以下に示すような方法がある。図３０は、サーチ・アライン・アプローチ処理方法を示すフローチャートである。
図３０に示すように、サーチ・アライン・アプローチが開始されると、サーチ動作（１）を実行する（ステップＳ３２）。サーチ動作（１）では、首を振りできるだけ広い範囲の情報を集める動作とする。その結果、周囲に昇降可能な階段があるかどうかを判断する（ステップＳ３２）。ここで、検出された階段の中で１段目の踏面を構成する平面の高さｎｚを利用し、高さがstep_min_z＜ｎｚ＜step_max_zを満たす場合に、昇降可能であると判断する。昇降可能な階段があった場合（ステップＳ３２：Ｙｅｓ）、階段を近くで認識しなおすために、階段に対して決められた距離（align_distance）まで移動するアライン動作を実行し（ステップＳ３３）、昇降しようとしている階段を再度認識する（ステップＳ３４）。このステップＳ３４の動作がサーチ動作（２）である。
そして、昇降可能な階段であるか否かを再度確認し（ステップＳ３５）、サーチ動作（２）が成功している場合には、再認識した階段に対して対峙しかつ所定の距離のアライン位置に移動完了できているか否か、すなわちステップＳ３３のアライン動作が成功しているか否かを確認し（ステップＳ３６）、昇降可能な階段があり、かつアラインされている場合には（ステップＳ３５、Ｓ３６：Ｙｅｓ）、初段の階段のフロントエッジまで進むアプローチ動作を実行する（ステップＳ３７）。一方、ステップＳ３５にて昇降可能な階段がない場合にはステップＳ３１の処理に戻り、ステップＳ３６にてアライン動作が成功していない場合にはステップＳ３３の処理から繰り返す。
次に、ステップＳ２２における階段昇降動作について説明する。階段昇降動作は、一段上又は下の段（以下、次段という。）を現在の踏面からロボット装置自身が認識できる場合の昇降動作処理１と、現在の移動面から次段は認識できない場合等であって、２段以上、上の段又は下の段（以下、２段以上先の段という。）を認識できる場合の昇降動作処理２と、複数段の踏面が認識可能な場合の昇降動作処理３とがある。図３１、図３３、図３４は、それぞれ昇降動作処理１〜３の処理方法を示すフローチャートである。ここで、以下の説明においては、現在移動中の段（床面を含む）をstep-0、次段の段をstep-1、その更に次の段をstep-2、ｍ段先の段をstep-mとする。
先ず、次段（step-1）の踏面を観測・認識できる場合の昇降動作処理について説明する。図３１に示すように、昇降動作処理１が開始されると、階段を登る／降りる動作（クライム動作（１））を実行する（ステップＳ４１）。このクライム動作（１）では、上述のステップＳ３２において、階段の高さｎｚを認識できているため、この高さｎｚの正負の判断をすることにより、昇降モードを切り替える。すなわち、ｎｚ＜０であれば降りる動作であり、ｎｚ＞０であれば登る動作となる。登る場合と降りる場合では、後述するように、クライム動作に使用する制御パラメータの値が異なる。すなわち、制御パラメータを切り替えるのみで、登る動作と降りる動作を切り替え実行することができる。
そして、クライム動作（１）が成功したか否かを判断し（ステップＳ４２）、成功した場合（ステップＳ４２：Ｙｅｓ）は、サーチ動作（３）を実行する（ステップＳ４３）。このサーチ動作（３）は、ステレオビジョンシステムが搭載された頭部ユニットを動かし、周囲の距離データを取得して次の階段を検出する処理であり、サーチ動作（２）などとほぼ同様の動作処理である。
そして、ステップＳ３５と同様、サーチ動作（３）が成功したら（ステップＳ４４：Ｙｅｓ）、再びステップＳ４１の処理を繰り返す。階段が検索されなかった場合（ステップＳ４４：Ｎｏ）、階段を登り切ったか、又は下りきったものと判断し、階段面の中央など所定の位置まで移動するなどのフィニッシュ動作を実行し（ステップＳ４５）、処理を終了する。
次に、ステップＳ４１のクライム動作（１）について詳細に説明する。図３２は、ロボット装置が認識しているか又は認識する予定の階段面を示す模式図である。図３２に示すように、例えば、現在移動中のロボット装置の足底１２１Ｌ／Ｒが踏面１８１にあるとする。また、この図３２においては、フロントエッジＦＥ及びバックエッジＢＥに挟まれる安全領域及びこれに隣接する左右のマージンＭ１，Ｍ２を踏面と認識することとする。このとき、ロボット装置は、次の次段（step-2）の踏面１８２を認識できている。ここで、踏面１８１と踏面１８２との間にはその蹴り上げなどによりギャップ１８４が存在するものとする。昇降動作（１）では、現在の段（step-0）の踏面１８１から次の段（step-1）の踏面１８２に移動（クライム動作）可能であるかを判断するが、例えば、以下の基準を満たすものを移動可能と判断するものとする。  
５−１：次段（step-1）の踏面１８２のフロントエッジＦＥ１に十分近く、角度のずれが所定の閾値以下
５−２：次段（step-1）の踏面１８２の大きさが十分に大きい
５−３：フロントエッジＦＥ１から足底１２１Ｌ／Ｒの後端までの距離front_xが指定された昇降モードにおける制御パラメータfront_x_limitより大きい
５−４：バックエッジＢＥ１から足底１２１Ｌ／Ｒの前端までの距離back_xが昇降モードにおける制御パラメータback_x_limitより大きい  
ここで、更に次段（step-2）の踏面１８３を観測できた場合、次段（step-1）の踏面１８２の参照点における高さｚ１と更に次の段（step-2）の踏面１８３の参照点における高さｚ２の差（ｚ２−ｚ１）から、次段（step-1）の踏面１８２からその次の段（step-2）の踏面１８３へのクライム動作が登りであるか下りであるかを判断することができる。なお、２段先の段（step-2）の踏面１８３が認識できない場合は、現在の昇降状態が維持されるものとすればよい。
上記の５−１〜５−４からクライム動作可能と判断した場合は、階段を登る又は降りるクライム動作を実行する。ロボット装置は、現在の段（step-0）の踏面１８１において、踏面１８１のバックエッジＢＥ０にアラインしている。ここで、踏面１８１と踏面１８２との間のギャップ１８４が大きい場合には、次段（step-1）の踏面１８２のフロントエッジＦＥ１にアラインする動作をした後に、次段に移動し、そのバックエッジＢＥ１にアラインする。そして、次のクライム動作にて、次の段（step-2）の踏面１８３のフロントエッジＦＥ２にアラインし、踏面１８３に移動し、そのバックエッジＢＥ２にアラインする。すなわち、例えば、次の段の踏面のフロントエッジＦＥにアラインして、昇降動作し、その移動した踏面のバックエッジＢＥにアラインするまでをクライム動作とする。
また、ギャップ１８４が小さい場合には、現在の段（step-0）の踏面１８１のバックエッジＢＥ０と次段の踏面１８２におけるフロントエッジＦＥ１がほぼ一致するものとして、何れか一方のエッジにのみアライン動作するようにしてもよい。すなわち、現在の踏面１８１のバックエッジＢＥ０にアラインして次段の踏面１８２に移動し、そのバックエッジＢＥ１にアラインするか、次段の踏面１８２のフロントエッジＦＥ１にアラインして次段の踏面に移動し、更に次の段の踏面１８３のフロントエッジＦＥ１にアラインすればよい。この場合、クライム動作とは、次段のフロントエッジＦＥにアランする処理を省略し、次段に移動して移動した踏面のバックエッジＢＥにアライン動作を実行する処理となる。
以上の昇降動作処理１は、ロボット装置が階段を昇降動作中に次に移動可能な段（step-1）の踏面を観測できる場合に適用することができる。例えば２足歩行ロボット装置であれば、自身の足元を見下ろすことが可能なステレオビジョンシステム１を搭載しておく必要がある。
次に、次段の踏面を観測・認識可能な場合の昇降動作処理１とは異なり、ロボット装置の頭部ユニットと体幹部ユニットとの接続部の可動角の制約などにより、現在の段（step-0）における踏面から次段（step-1）の踏面を観測・認識することができず、２段先（step-2）又はそれ以上先の段（step-m）の段の踏面を認識可能な場合の昇降動作処理２、３について説明する。先ず、２段先の段（step-2）の踏面を認識可能な場合について説明すると、図３２に示すように、昇降動作処理２が開始されると、上述と同様に、サーチ動作（４）を実行する（ステップＳ５１）。このサーチ動作（４）では、ステレオビジョンシステム１により２段先の段（step-2）の踏面を観察・認識する。このサーチ動作（４）は、２段先の段（step-2）の踏面を認識する以外、上述のステップＳ４３などと同様の処理である。
そして、クライム動作（２）を実行する（ステップＳ５２）。クライム動作（２）は、クライム動作（１）と同様の動作である。この場合においても、クライム動作における階段を登る動作と降りる動作の切り替えは、同じく次段（step-1）の踏面の高さｎｚにより判断することができる。次段（step-1）の踏面とは、現在の段（step-0）の踏面より時間的に前に移動していた段の踏面において観測された踏面である。
そして、クライム動作が成功した場合（ステップＳ５３：Ｙｅｓ）、ステップＳ５１にてサーチした結果、２段先の階段（step-2）が検出されていれば（ステップＳ５４：Ｙｅｓ）、それを次の階段として更新し（step-1＝step-2）（ステップＳ５６）、ステップＳ５１からの処理を繰り返す。ステップＳ５１のサーチ動作（４）にて２段先の段（step-2）の踏面が検出されていない場合（ステップＳ５５：Ｎｏ）、フィニッシュ動作を実行し（ステップＳ５５）、処理を終了する。
なお、ここでは２段先の段の踏面を観測できることとして説明したが、３段以降先の踏面を観測できる場合であっても同様にして処理すればよい。
次に、複数段（以下、ｍ段）先までの踏面が観測・認識可能な場合についの昇降動作を昇降動作３として説明する。
図３４に示すように、１〜ｎ段が観測・認識できている場合に昇降動作を行う場合、まず、サーチ動作（５）を実行する。サーチ動作（５）は、基本的には、ステップＳ５１と同様であるが、認識可能なｍ段先の段（step-m）までの踏面を観測対象としている点が異なる。
そして、クライム動作（３）をｋ段分実行する（ステップＳ６２）。クライム動作（３）においても、現在までに観測されている複数の踏面の高さの差から昇降モードを決定することができる。すなわち、ｉ番目、ｉ−１番目の踏面の高さｚｉ−ｚｉ−１が負であれば階段を下りる動作となり、０又は正であれば階段を登る動作モードとすればよい。このクライム動作（３）において移動する踏面の情報は、現在の段より時間的にｍ段前にて観測済みのデータである。
そして、昇降動作が成功した場合（ステップＳ６３：Ｙｅｓ）、現在の踏面より先の階段データが観測できたか否か、すなわちｍ＋ｎ＞ｋであるか否かが判定され（ステップＳ６４）、ｍ＋ｎ＞ｋであれば（ステップＳ６４：Ｙｅｓ）、step-(ｋ+1)〜step-(n+m)を、step-1〜step-(ｍ＋ｎ−ｋ)（ｎ＝ｍ）として、更新し（ステップＳ６６）、ステップＳ６１からの処理を繰り返す。一方、ｍ＜０であれば（ステップＳ６４：Ｎｏ）、次に移動する対象となる踏面が存在しないため、フィニッシュ動作を実行し（ステップＳ６５）、処理を終了する。
以上説明したように、階段昇降動作処理１〜３においては、クライム動作における登る動作と降りる動作とで使用する制御パラメータを変更するのみで、階段を登ることも、降りることも同様の手順にて実行することができる。階段昇降動作に使用する制御パラメータは、ロボット装置の足底の現在の踏面に対する位置を規制するためのものである。
図３５Ａは、ロボット装置により認識されている踏面と足底の関係を説明するための図、図３５Ｂは、クライム動作に使用する制御パラメータの一例を示す図である。
図３５Ａに示す各制御パラメータは、以下を示す。
step_min_z：現在の段と次段の段との高さの差（蹴り上げ）の昇降可能最小値
step_max_z：現在の段と次段の段との高さの差（蹴り上げ）の昇降可能最大値
ａｄ（align_distance）：アライン位置におけるフロントエッジＦＥとロボット装置との間の距離
ａｘ（approach_x）：アプローチ位置におけるフロントエッジＦＥとロボット装置との間の距離
front_x_limit：踏面におけるフロントエッジＦＥと足底１２１の後端部との距離の限界値（minimal x-value）
back_x_limit：踏面におけるバックエッジＢＥと足底１２１の前端部との距離の限界値（maximal x-value）
back_x_desired：バックエッジＢＥと足底１２１の前端部との距離の欲求値(desired value)  
階段を登る場合には、蹴り上げがstep_min_zより小さい場合は段差がある（階段）と見なさないものとし、蹴り上げがstep_max_zより大きい場合には、階段昇降不可と判断する。同様に、階段を降りる場合には、蹴り上げがstep_max_zより大きい場合には、階段と見なさないものとし、step_min_zより大きい場合には、階段昇降不可と判断するものとする。
align_distanceは、アライン動作をする場合にのみ使用するパラメータであり、階段を登る／降りる動作を実行する階段昇降動作処理を開始する場合、すなわち、初段の段に対する昇降動作を行う場合に使用される。同様に、approach_xも、アプローチ動作をする場合にのみ使用するパラメータであり、階段を登る／降りる動作を実行する階段昇降動作処理を開始する場合に使用される。
front_x_limit及びback_x_limitは、ロボット装置が認識している踏面と、ロボット装置の足底の関係を規定するもので、踏面のバックエッジＢＥ及びフロントエッジＦＥと、足底の端部との距離、すなわちその踏面に移動した場合における踏面のあまり部分がこれらの値より小さい場合には、その踏面に移動できない、又は移動できたとしても次の昇降動作が不能であると判断される。ここで、登り動作において、front_x_limit及びback_x_limitの値が何れも負であることは、踏面が足底より小さいことを許すことを示す。すなわち、登り動作においては、踏面は足底より小さい場合であっても移動可能と判断することができる。
back_x_desiredは、現在の踏面におけるバックエッジＢＥに対してロボット装置がアラインしたい位置におけるバックエッジＢＥと足底前端部との距離を示すものであり、図３５Ｂに示すように、通常、登る場合であれば、back_x_desiredは、バックエッジＢＥよりも手前、本実施の形態においては、バックエッジＢＥから１５ｍｍ手前の位置となっており、一方、降りる動作であれば、バックエッジＢＥより足底がはみ出した位置、本実施の形態においては、５ｍｍはみ出した位置となる。これは、登る動作であれば、次段に足上げして移動するまでにある程度の距離が必要であるのに対し、降りる動作においては、そのような距離が不要であると共に、踏面からはみ出すような位置の方が次段又はそれ以降の踏面の観測・認識が容易となるためである。
図３６及び図３７は、図３５に示す制御パラメータを使用して実際にロボット装置が昇降動作を行った様子を撮影したものをトレースした図である。図３６は、ロボット装置が階段を登る動作を示している。最上段から番号順に、ステップＳ３１のサーチ動作（１）実行の様子（Ｎｏ．１）、ステップＳ３３のアライン動作実行の様子（Ｎｏ．２）、ステップＳ３２のサーチ動作（２）実行の様子（Ｎｏ．３）、ステップＳ３７のアプローチ動作実行の様子（Ｎｏ．４）、ステップＳ５１のサーチ動作（４）実行の様子（Ｎｏ．５）、ステップＳ５２のクライム動作（２）実行の様子（Ｎｏ．６）、ステップＳ５２のクライム動作（２）の続きであって現在の踏面のバックエッジＢＥにアラインしている様子（Ｎｏ．７）、ステップＳ５１のサーチ動作（４）実行の様子（Ｎｏ．８）、・・・を示し、サーチ動作（４）をして次段の踏面が観測されなかった場合（Ｎｏ．１７）に、階段昇降動作終了（フィニッシュ）動作を行っている様子（Ｎｏ．１８）を示す。
図３７は、降りる動作を示すものであり、図３６の登る動作と同様に、サーチ動作（Ｎｏ．１、Ｎｏ．４、Ｎｏ．７、Ｎｏ．１０、Ｎｏ．１３、Ｎｏ．１６）、クライム動作（アライン動作含む）を繰り返し（Ｎｏ．５、Ｎｏ．６、Ｎｏ．８、Ｎｏ．９、Ｎｏ．１１、Ｎｏ．１２、Ｎｏ．１４、Ｎｏ．１５）、次段の踏面が観測されなくなった時点でフィニッシュ動作を行って（Ｎｏ．１８）、昇降動作を終了する。
次に、本実施の形態における変形例について説明する。以上の説明においては、階段を登る動作、降りる動作について説明したが、本実施の形態の制御方法に係るアルゴリズムを適用すれば、複数の段からなる階段のみならず、１段のみからなる段部や、１段の凹部が存在しても、その段差が所定の値以下であれば、移動を可能にするものである。
先ず、単一の段部に登る動作について説明する。図３８は、単一の段部とロボット装置の足底の関係を示す図である。図３８において、１９１で示すのは、床面（ｚ＝０）からの高さがｚ１＝３０である段部を示し、ここでは、ロボット装置が次段（step1）となる段部１９１の紙面下側から上側に移動する場合について説明する。段部１９１に対し紙面下側における移動面の高さがｚ０＝０、紙面上側における移動面の高さがｚ２＝０である場合であって、現在、高さｚ０の移動面を移動しているとした場合、ｚ１−ｚ０＝３０＞０であるので次段の段部１９１への移動は登り動作と判断し、ｚ２−ｚ１＝−３０＜０であるので段部１９１から次の領域への移動は降りる動作であると判断することができる。この判断に応じてクライム動作において、上述した制御パラメータの値を変更すればよい。ここで、図３５Ｂに示す制御パラメータを使用した場合、登る動作の場合のront_x_limit及びback_x_limitは、何れも負の値であって、ロボット装置の足底１２１が図３８に示すように、段部１９１からはみ出した状態であっても移動可能と判断することを示す。
次に、単一の窪み（凹部）に降りる動作について説明する。図３９は、単一の凹部とロボット装置の足底の関係を示す図である。図３９において、１９２で示すのは、床面（ｚ＝０）からの高さがｚ１＝−３０である凹部を示し、ここでは、ロボット装置が次段（step1）となる凹部１９２の紙面下側から上側に移動する場合について説明する。凹部１９２に対し紙面下側における移動面の高さがｚ０＝０、紙面上側における異動面の高さがｚ２＝０である場合であって、現在、高さｚ０の移動面を移動しているとした場合、ｚ１−ｚ０＝−３０＜０であるので次段の凹部１９２への移動は降りる動作と判断し、ｚ２−ｚ１＝３０＞０であるので凹部１９２から次の領域への移動は登る動作であると判断することができる。したがって、段部１９１と同様、この判断に応じてクライム動作において、上述した制御パラメータの値を変更すればよい。ここで、図３５Ｂに示す制御パラメータを使用した場合、降りる動作の場合のront_x_limit及びback_x_limitは、何れも正の値であって、ロボット装置の足底１２１が図３９に示すように、凹部１９１より小さい場合にのみ移動可能と判断する。
本実施の形態においては、検出された平面から例えば水平など、移動可能と判断できる平面を抽出し、その領域を含む多角形から階段の踏面を認識する。そして、多角形のフロントエッジＦＥ及びバックエッジＢＥなどの踏面に関する情報と、その床面からの高さを含む階段情報を利用して階段昇降動作を行う。昇降動作においては、移動する踏面をサーチ動作を実行し、サーチできた踏面のフロントエッジＦＥ又は現在の移動面におけるバックエッジＢＥに対してアライン動作を実行し、次段の移動面と現在の移動面との高さの違いから登る動作か降りる動作かを判断して制御パラメータを切り替えることにより、通常の矩形の踏面からなる階段のみならず、スパイラル形状の階段などの昇降動作が可能となると共に、登る動作も降りる動作も制御パラメータを変更するのみで同一の手順にて実行することができる。したがって、階段のみならず、単一の段部や、単一の凹部などへの移動も同一の制御方法にて移動可能となる。
また、階段認識においては、認識した階段を時間的に統合していくため、例えばロボット装置の大きさに対して階段が大きい、ロボット装置に搭載されるステレオビジョンシステムの位置の制約などの理由で視野が限られたロボット装置などにおいても、高域に亘って階段情報を認識することができる。また、この階段情報を利用して昇降動作する際、同じくステレオビジョンシステムの位置の制約などの理由で次段の踏面が観測・認識できない場合であっても、過去に観測・認識してある階段情報を利用して昇降動作を同様に行うことができる。
本変形例における平面検出装置は、線分拡張法により、視野内において支配的な平面だけでなく、例えば階段など複数の平面が存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさせることにより計測ノイズに対してロバストな平面検出結果を得ることができるものである。
図４０は、本変形例における平面検出装置を示す機能ブロック図である。図４０に示すように、平面検出装置１００は、３次元の距離データを取得する距離データ計測手段としてのステレオビジョンシステム（Stereo Vision System）１と、３次元の距離データからなる距離画像に存在する平面を線分拡張法により検出する平面検出部２とを有する。平面検出部２は、画像を構成する距離データ点から同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部２ａと、画像内に含まれる、線分抽出部２ａよって抽出された全線分からなる線分群から、該画像内に存在する１又は複数の平面領域を検出する領域拡張部２ｂとを有する。領域拡張部２ｂは、線分群から同一平面上に存在すると推定される任意の３本の線分を選択し、これらから基準平面を求める。そして、選択した３本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新すると共に基準平面の領域を拡張する。
線分抽出部２ａは、その距離画像における列または行毎の各データ列において、３次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて１以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群Ｄ１１を領域拡張部２ｂに出力する。
領域拡張部２ｂは、この線分群Ｄ１１において、同一の平面に属すると推定される線分を３本選択し、これらから基準平面としての種となる平面を求める。この種となる平面の領域（領域種：seed region）に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群Ｄ２を出力する。
ロボット装置２０１は、障害物回避や階段昇降など平面の情報が必要なとき、または定期的にこれらの処理を行うことによって、階段や床面、壁といった歩行に重要な平面の情報を取得する。
ここで、このような３次元距離データをステレオビジョンシステム１によって取得するためには、階段ＳＴ２の表面に模様（テクスチャ）が必要となる。すなわち、２台のカメラによる視差により得ることができるため、模様がないものは視差が算出できず、正確に距離を計測することができない。すなわち、ステレオビジョンシステムにおける距離データの計測精度は、計測対象のテクスチャに依存することになる。なお、視差とは、空間中のある点が左目及び右目に写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
そこで、図４１に示すように、ロボット装置の頭部ユニットに、ステレオビジョンシステムを構成するステレオカメラ１１Ｒ／Ｌを備えると共に、例えば同じく頭部ユニットなどに投射手段としての例えば赤外光などを出力する光源１２を設ける。この光源１２は、模様がない階段ＳＴ３、その他テクスチャがないか少ない物体、壁などの対象物に対してこれを投射（照射）し、ランダムなパターンＰＴを付与する模様付与手段として作用する。なお、ランダムパターンＰＴを形成して距離画像を取得できるものであれば、ランダムパターンＰＴを付与する手段は赤外光を投射する光源などには限らず、例えばロボット装置自ら対象物に模様を書いたりしてもよいが、赤外光であれば、人間の目にはみえないものの、ロボット装置に搭載されるＣＣＤカメラなどにおいては観測可能なパターンを付与することができる。
次に、平面検出装置１００の平面検出部２について説明する。この平面検出部２は、線分拡張法を使用して平面を検出するものであり、図４２は、線分拡張法による平面検出方法を説明する図である。線分拡張法による平面検出では、図４２に示すように、まず、焦点Ｆから撮影された画像１１において、行方向または列方向のデータ列における処理をする。画像内の例えば行方向の画素列（image row：イメージロウ）において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分からなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。
図４３は、線分拡張法による平面検出処理を示すフローチャートである。図４３に示すように、先ず、距離画像を入力し（ステップＳ７１）、距離画像の行方向（又は列方向）の各画素列において同一平面に属すると推定されるデータ点から線分を求める（ステップＳ７２）。そして、これらの線分群の中から同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める（ステップＳ７３）。このステップＳ７３では、まず、平面の種となる領域（以下、領域種（seed region）という。）を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向（又は左右隣接する列方向）の１ラインを含む３本の線分が同一平面にあることを条件とする。ここで、選択した３本の線分からなる領域種が属する平面を基準平面とし、３本の線分から平均して求まる平面を求めておく。また、３本の線分からなる領域を基準平面領域とする。
そして、選択した領域種に隣接する行方向（又は列方向）の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し（領域拡張処理）、追加した線分を含めたものとして上記基準平面を更新し（平面更新処理）、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域（３本の線分）が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群の中から同一平面を構成するものを連結する。そして、本変形例においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップＳ７４として更に設け、最終的な平面とするが、詳細は後述する。
ここで、３次元距離データから線分を検出し、これを同一平面毎にまとめた領域を１つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本変形例においては、ステップＳ７２における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しようとしても、距離データの精度に応じて閾値を変更しなければ、over-segmentation又はunder-segmentationなどもの問題が生じてしまう。そこで、本変形例においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。
以下、図４３に示す線分拡張法による平面検出方法について更に詳細に説明する。線分抽出部(Line Extraction)２ａは、上述したように、ステレオビジョンシステム１からの３次元距離画像を入力とし、距離画像の各列または各行毎に３次元空間内で同一平面上にあると推定される線分を検出する。この線分抽出において、計測ノイズなどによる、上述したover-segmentationやunder-segmentationの問題、すなわち、本来は複数の平面であるのに１つの平面として認識してしまったり、本来は１つの平面であるのに、複数の平面として認識してしまったりする問題を回避するため、データ点の分布に応じて適応的に線分フィッティングさせるアルゴリズム（Adaptive Line Fitting）を導入する。Adaptive Line Fittingは、線分抽出部２ａにおいて、先ず比較的大きい閾値を使用して大まかに第１の線分としての線分を抽出し、次に抽出された第１の線分に属するデータ点群から後述する最小二乗法によって得られる第２の線分としての線分に対する該データ点群の分布を解析する。すなわち、同一平面上に存在するか否かを大まかに推定してデータ点群を抽出し、抽出したデータ点群におけるデータ点の分布の偏りがあるか否かを解析して同一平面上に存在しているか否かを再度推定する。
本変形例においては、このデータ点の分布を解析し、データ点群が後述するジグザグ形（zig-zag-shape）に当てはまる場合には、分布に偏りがあるとしてデータ点群を分割する処理を行い、これを繰り返すことによって、データ点群に含まれるノイズに対して適応的に線分の抽出を行うアルゴリズムを使用するものとする。
図４４は、線分抽出部２ａにおける処理、すなわち、図４３におけるステップＳ７２の処理の詳細を示すフローチャートである。まず、線分抽出部２ａには、距離データが入力される。入力された距離データのうち、例えば行方向の画素列（データ点列）において、３次元空間上で同一平面上に存在すると推定されるデータ点群を抽出する。３次元空間上で同一平面上に存在すると推定されるデータ点群は、例えば隣接するデータ点の距離が、例えば６ｃｍ以下など、データ点間の３次元空間における距離が所定の閾値以下のものからなるデータ点の集合などとすることができ、これをデータ点群（Ｐ［0・・・n-1］）として抽出する（ステップＳ８１）。そして、このデータ点群Ｐ［0・・・n-1］に含まれるサンプル数ｎが処理に最低限必要なサンプル数（必要最小値）min_nより多いか否かをチェックし（ステップＳ８２）、データ数ｎが必要最小値min_nより少ない場合（Ｓ８２：ＹＥＳ）には、検出結果として空集合を出力して処理を終了する。
一方、サンプル数ｎが必要最小値min_n以上である場合（Ｓ８２：ＮＯ）、データ点群Ｐ［0・・・n-1］の一方の端点Ｐ［0］と他方の端点Ｐ［n-1］とを結ぶ線分（弦）Ｌ１を第１の線分として生成する。そして、データ点群Ｐ［0・・・n-1］から、この線分Ｌ１との距離が最も大きいデータ点を着目点brkとして検索し、その距離distを算出する（ステップＳ８３）。最大距離distがデータ点群分割の閾値max_dより大きい場合には（Ｓ８４：ＹＥＳ）、データ点群データ点群Ｐ［0・・・n-1］を着目点（分割点）brkにて２つのデータ点群ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する（ステップＳ８８）。
一方、最大距離distがデータ点群分割の閾値max_dより小さい場合には（Ｓ８４：ＮＯ）、データ点群Ｐ［0・・・n-1］から後述する最小二乗法によって最適な線分の方程式lineを求め（ステップＳ８５）、この方程式lineが示す線分Ｌ２を第２の線分として生成する。そして、データ点群Ｐ［0・・・n-1］がこの線分Ｌ２に対して後述するZig-Zag-Shapeであるかどうかを調べる（ステップＳ８６）、Zig-Zag-Shapeでない場合（Ｓ８６：ＮＯ）、得られた線分の方程式lineを線分抽出結果リストに追加し（ステップＳ８７）、処理を終了する。
また、ステップＳ８６においてステップＳ８５で求めた線分がZig-Zag-Shapeである判断された場合（Ｓ８６：ＹＥＳ）、上述のステップＳ８４と同様、ステップＳ８８に進み、ステップＳ８３において距離distを求めた着目点brkにてデータ点群を２つのデータ点群Ｐ［0・・・brk］及びＰ[brk・・・n-1]に分割する。このステップＳ８８にて２つのデータ点群が得られた場合には、それぞれを再帰的に再度ステップＳ８１からの処理を行う。そして、この処理を分割された全てのデータ点について分割されなくなるまで、すなわち全てのデータ点群がステップＳ８７を経るまで処理を繰り返し、これにより、全ての線分が登録された線分抽出結果リストを得る。このような処理によって、データ点群Ｐ［0・・・n-1］からノイズの影響を排除し複数の線分からなる線分群を精度よく検出することができる。
なお、ステップＳ８３にてデータ点群Ｐ［0・・・n-1］の端点を結ぶ線分Ｌ１を生成するものとして説明したが、例えばデータ点群Ｐ［0・・・n-1］の分布、性質など必要に応じてデータ点群Ｐ［0・・・n-1］から最小二乗により線分Ｌ１を求めてもよい。また、本変形例においては、着目点brkは、端点を結んだ線分Ｌ１との距離が最大の点１つとしているが、例えば、上記のように最小二乗により求めた線分との距離が最大の点としたり、距離がデータ点群分割の閾値max_d以上のものが複数ある場合はそれら全ての点又は選択した１つ以上にてデータ点群Ｐ［0・・・n-1］を分割するようにしてもよい。
次に、ステップＳ８５における最小二乗による線分生成方法（Least-Squares Line Fitting）について説明する。あるｎ個のデータ点群Ｐ［0・・・n-1］が与とき、データ点群に最もフィットした直線の方程式を求める方法を示す。直線の方程式のモデルを下記式（１）で表す。
【数１】
0004618247.tif 000002
この場合、ｎ個のデータ点群Ｐ［0・・・n-1］の１点（ｘｉ,ｙｉ）において、直線方
程式のモデルとデータ点との誤差の総和は下記式（２）で表すことができる。
【数２】
0004618247.tif 000003
データ点群に最もフィットした直線は、上記式（２）の誤差の総和を最小化することによって求められる。上記式（２）を最小にするα及びｄは、データ点群Ｐの平均及び分散共分散行列を用いて下記（３）のように求めることができる。
【数３】
0004618247.tif 000004
次に、ステップＳ８６におけるジグザグ形（Zig-Zag-Shape）判別方法について説明する。このZig-Zag-Shape判別では、あるｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α，ｄ）、ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、そのデータ点群Ｐ［0・・・n-1］が、図４５Ａに示すように直線Lineに対して交差するか、図４５Ｂに示すように、例えばノイズなどの影響によりデータ点が一様に分布しているかを判別するものである。基本的には、直線Lineの一方にデータ点群Ｐ［0・・・n-1］が連続して現れる数をカウントし、ある一定数を超えて連続して現れる場合には、zig-zag-shapeであると判断することができる。図４５Ａの場合には、データ点群Ｐ［0・・・n-1］によりよくフィットする直線Lineを求めるためにデータ点群Ｐ［ｉ］を分割する必要がある。図４６は、Zig-Zag-Shape判別方法を示すフローチャートである。
まず、データ点群Ｐ［0・・・n-1］と直線Line（α,ｄ，σ）とを入力する（ステップＳ９０）。ここで、σは、点列の標準偏差を示す。次に、この標準偏差σが所定の閾値th_σより大きいか否かを判断する。この標準偏差σが閾値th_σより小さい場合（ステップＳ９１：Ｎｏ）は、演算器の浮動小数点演算誤差による誤差検出の影響を回避するため、判別を終了する。そして、標準偏差σが閾値th_σより大きい場合のみ判別処理を継続する。次に、データ点群Ｐ［0・・・n-1］のうちの最初のデータ点Ｐ［0］が直線のどちら側にあるかをsing(sdist(P[0]))によって判断し、この結果をval0に代入すると共にval0と同じ側にあるデータ点の連続数をカウントするカウンタ（以下、連続点カウンタといい、このカウント値をカウント値countという。）のカウント値countを１に設定する（ステップＳ９２）。ここで、sign(x)は、ｘの値の符号（＋又は−）を返す関数であり、sdist(i)は、P[i].xcosα+ P[i].ycosα+dとして計算された直線Lineにおいて、ｉ番目のデータ点との正負の距離を示す。すなわち、Val0には、データ点Ｐ［0］が直線Lineのどちら側にあるかで＋又は−の符号が代入される。
次に、データ点をカウントするためのカウンタ（以下、データ点カウンタといい、このカウント値をカウント値ｉという。）のカウント値iを１とする（ステップＳ９３）。そして、データ点カウンタのカウント値ｉがデータ数ｎより小さい場合（ステップＳ９４：ＹＥＳ）、その次のデータ（以下、ｉ番目とする。）のデータ点であるデータ点Ｐ［ｉ］が直線のどちら側にあるかをsing(sdist(P[i]))によって判断し、この結果をvalに代入する（ステップＳ９５）。そして、ステップ９２にて求めたval0とステップＳ９５にて求めたvalとを比較し、val0とvalとが異なる場合（ステップＳ９６：ＮＯ）、val0にvalを代入し、連続点カウンタのカウント値countに１を代入し（ステップＳ９８）、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ１００）ステップＳ９４からの処理に戻る。
一方、ステップＳ９６において、val0とvalとが同じ場合（ステップＳ９６：ＹＥＳ）、データ点Ｐ［ｉ−１］とＰ［ｉ］は、直線Lineに対して同じ側にあると判断され、連続点カウンタのカウント値countを１つインクリメントする（ステップＳ９７）。更に、連続点カウンタのカウント値countがZig-Zag-Shapeと判定されるための最小のデータ点数min_cより大きいか否か判定し（ステップＳ９９）、大きい場合には（ステップＳ９９：ＹＥＳ）、Zig-Zag-Shapeと判断し、TRUEを出力して処理を終了する。一方、連続点カウンタのカウント値countが最小のデータ点数min_cより小さい場合には（ステップＳ９９：ＮＯ）、ステップＳ１００に進み、データ点カウンタのカウント値ｉをインクリメントして（ステップＳ１００）、ステップＳ９４からの処理を繰り返す。
そして、このステップＳ９４からの処理を、データ点カウンタのカウント値ｉがデータ点数ｎに到達するまで続け、カウント値ｉ＞＝ｎとなったところで、FALSEを出力して処理を終了する。
このようなジグザク形判別処理によって、ｎ個のデータ点群Ｐ［0・・・n-1］と直線Line（α,ｄ）：ｘｃｏｓα＋ｙｃｏｓα＋ｄ＝０が与えられたとき、このデータ点群が直線Lineに対してzig-zagに交差しているかどうかを判断することができる。これによって、上述したように、ステップＳ８６にてデータ点群を分割するべきかどうかを判断することができ、最小二乗により求めた直線に対し、データ点群がzig-zagに交差していると判断した場合にはデータ点群を分割すべきと判断してステップＳ８８の処理へ進み、着目点brkを分割点としてデータ点群を分割することができる。なお、上記ステップＳ９１〜ステップＳ１００までの処理は図４７のように表現することも可能である。
また、このようなZig-Zag-Shape判別処理は、演算器のみならずハードウェアで行うことも可能である。図４８は、Zig-Zag-Shape判別処理を行う処理部を示すブロック図である。図４８に示すように、Zig-Zag-Shape判別処理部２０は、ｎ個のデータ点群Ｐ［0・・・n-1］が入力され、順次各データ点Ｐ[ｉ]が直線Lineの何れ側に位置するかを判別し、その判別結果Valを出力する方向判別部２１と、１つ後のデータと方向判別部２１の結果を比較させるための遅延部２２と、データ点Ｐ[ｉ]における方向判別結果Valとデータ点Ｐ[ｉ−１]における方向判別結果Val0とを比較する比較部２３と、比較部２３においてVal＝Val0の場合に、カウント値をインクリメントする連続点カウンタ２４と、連続点カウンタ２４のカウント値countと最小データ点数格納部２６から読み出した最小データ点数min_cとを比較する比較部２５とを有する。
このZig-Zag-Shape判別処理部における動作は以下のようになる。すなわち、方向判別部２１は、データ点群Ｐ［0・・・n-1］から最小二乗法により直線Lineを求め、各データ点Ｐ[ｉ]と直線Lineとの正負の距離を求め、その正負の符号を出力する。遅延部２２は、データ点Ｐ[ｉ−１]の直線Lineまでの距離に対する正負の符号が入力されると１つ後のデータ点Ｐ[ｉ]の正負の符号が入力されるタイミングまでデータを格納する。
比較部２３は、データ点Ｐ[ｉ]とデータ点Ｐ[ｉ−１]の上記正負の符号を比較し、同じ符号である場合にはカウンタ２４のカウント値countをインクリメントする信号を出力し、正負の符号が異なればカウント値countに１を代入する信号を出力する。比較部２５は、カウント値countと最小データ点数min_cとを比較し、最小データ点数min_cよりカウント値countが大きい場合には、データ点群Ｐ［0・・・n-1］がジグザグであることを示す信号を出力する。
次に、図４０に示す領域拡張部（Region Growing）２ｂについて説明する。領域拡張部２ｂは、線分抽出部２ａによって得られた線分群を入力とし、それらの線分それぞれがどの平面に属しているかを点列の平面への当てはめ（Plane Fitting）により判断し、与えられる線分群からなる領域を複数の平面（平面領域）に分離する。複数の平面に分離するために、以下の手法をとる。
先ず、与えられた線分群から、同じ平面上にあると推定される隣接する３本の線分を検索する。この３本の線分により求められる平面（基準平面）が、平面の種となるものであり、この３本の線分が含まれる領域を領域種（seed region）という。そして、この領域種に隣接する線分を順次、基準平面と同一平面上にある線分か否かを点列の平面への当てはめ（Plane Fitting）により判断し、隣接する線分が同じ平面に含まれると判断された場合には、この線分を領域拡大用の線分として領域種に追加してその領域を拡大すると共に、基準平面の方程式を上記領域拡大用の線分を含めて再度算出し直す。このような処理によって、全ての線分を何れかの領域（平面）に配分する。
図４９は、領域拡張処理を説明するための模式図である。図４９に示すように、画像３０内に複数の平面からなる階段３１が存在する場合、例えば太線で示す３２ａ〜３２ｃの３本の線分が領域種として選択されたとする。これら３本の線分３２ａ〜３２ｃからなる領域が領域種となる。先ず、この３つの線分３２ａ〜３２ｃにより１つの平面（基準平面）Ｐを求める。次に、領域種の最も外側の線分３２ａ又は３２ｃに領域種外にて隣接するそれぞれデータ列３３又は３４において、平面Ｐと同一の平面である線分を選択する。ここでは、線分３３ａが選択されるとする。次に、これら４本の線分群からなる平面Ｐ’を求め、基準平面Ｐを更新する。次に、線分３４ａが選択されれば、５本の線分群からなる平面Ｐ’’を求め、平面Ｐ’を更新する。これを繰り返すことにより、階段３１の２段目の踏面が、破線で囲まれる平面４５として求められる。このようにして、選択された領域種を種として追加する線分がなくなるまで領域拡大処理する。そして、追加する線分がなくなった場合、再び画像３０内から領域種となる３つの線分を検索して領域拡大処理を実行するというような処理を繰り返し、領域種となる３つの線分がなくなるまで図４３のステップＳ３の処理を繰り返す。
次に、データ点群Ｐ［0・・・n-1］から構成される平面の方程式を推定する手法（Plane Fitting）、これを使用して領域種を選択する方法（Selection of seed region）、領域種から領域を拡大していく領域拡張処理（Region growing）、及び得られた平面方程式から誤差が大きいものなどを除いて再度算出する後処理（Post processing）について説明する。
３次元空間内の点ＰはＰ＝（ｘｉ，ｙｉ，ｚｉ）により表され、平面の方程式はその法線ベクトルｎ（ｎｘ，ｎｙ，ｎｚ）と非負の定数ｄによって下記式（４）で表される。
【数４】
0004618247.tif 000005
ここで、ステレオカメラでは、焦点を通る平面を観測することができない、すなわち、平面は焦点を通らないため、ｄ≠０とすることができる。したがって、平面は、最小二乗法により下記式（５）に示す値を最小にする値として求めることができる。
【数５】
0004618247.tif 000006
最適解はｎ＝ｍ／‖ｍ‖，ｄ＝−１／‖ｍ‖として求まる。ここで、‖・‖は、ベクトルの大きさ、ｍは、行列式によって連立一次方程式を解くクラメールの法則（Cramer’s rule）を使用して下記（６−１）のように容易に得られる線形システムの解である。
【数６】
0004618247.tif 000007
この解は、新たなデータ点が加えられたり、又はデータ点削除されたりした場合であっても、上記式（６−２）に示すＡとｂの値を更新するのみで、平面パラメータを再計算することができる。更に、本変形例における線分抽出方法の場合はｎ個のデータ点群の２つのモーメント（１次モーメント：平均、２次モーメント：分散）Ｅ（ｐ）、Ｅ（ｐｐＴ）が既知であり、これらを使用して、下記（７）に示すようにＡ，ｂを更新することができ、ｎ個のデータ点群における平面更新処理に拡張することができる。
【数７】
0004618247.tif 000008
また、一度平面パラメータｎ，ｄを算出すれば、求まった平面方程式から、ｎ個のデータ点群の平面方程式からの外れ度合いを示す平面方程式の２乗平均誤差（ＲＭＳ（root mean square）residual）（以下、ｒｍｓという。）を下記式（８）により算出することができる。この場合も、ｎ個のデータ点の上記２つのモーメントを使用して下記式（８）を求めることができる。
【数８】
0004618247.tif 000009
上記（８）に示すように、各データ点が求めた平面上にあれば平面方程式の２乗平均誤差ｒｍｓ（ｐ１・・・ｐｎ）は０になる値であり、この値が小さいほど各データ点が平面によくフィットしていることを示す。
次に、領域種（seed region）を検索する方法及び領域種から領域を拡大すると供に平面を更新する方法について説明する。図５０は、領域種を検索する処理及び領域拡張処理の手順を示すフローチャートである。図５０に示すように、領域種の選択には、先ず、線分抽出の際に使用した行方向又は列方向のデータ列が隣接する３つの線分（ｌ１，ｌ２，ｌ３）であって、互いの線分（ｌ１，ｌ２），（ｌ２，ｌ３）における画素位置が上記データ列とは直交する方向にて重複したものを検索する（ステップＳ１０１）。各データ点は画像内における画素位置を示すインデックス（index）を有しており、例えば行方向のデータ列における線分である場合、このインデックスを比較して列方向にて重複しているか否かを比較する。この検索に成功した場合（ステップＳ１０２：ＹＥＳ）、上記式（７）を使用して上記（６−１）を算出する。これにより、平面パラメータｎ，ｄを決定でき、これを使用して上記式（８）に示す平面方程式の２乗平均誤差（ｌ１，ｌ２，ｌ３）を計算する（ステップＳ１０３）。そして、この平面方程式の２乗平均誤差ｒｍｓ（ｌ１，ｌ２，ｌ３）が例えば１ｃｍなどの所定の閾値ｔｈｒｍｓ１より小さい場合には、この３つの線分を領域種として選択する（ステップＳ１０４）。所定の閾値ｔｈｒｍｓ１より大きい場合には、再びステップＳ１０１に戻り、上記条件を満たす線分を検索する。また、領域種に選ばれた線分は、線分群のリストから除くことで、他の平面拡張などの際に使用されないようにしておく。
こうして選択された領域種から線分拡張法により領域を拡張する。すなわち、先ず、領域種の領域に追加する候補となる線分を検索する（ステップＳ１０５）。なお、この領域は、領域種が既に更新されている場合の、後述する更新された領域種も含む。候補となる線分は、領域種の領域に含まれる線分（例えばｌ１）に隣接する線分（ｌ４）であって、上述同様、これらの線分の画素位置が相互に重なりあうことを条件とする。検索が成功した場合（ステップＳ１０６：ＹＥＳ）、その平面方程式の２乗平均誤差ｒｍｓ（ｌ４）を算出し、これが所定の閾値ｔｈｒｍｓ２より小さいか否かを判定し（ステップＳ１０７）、小さい場合には平面パラメータを更新し（ステップＳ１０８）、再びステップＳ１０５からの処理を繰り返す。ここで、候補となる線分がなくなるまで処理を繰り返し、候補となる線分がなくなったら（ステップＳ１０６：ＮＯ）、ステップＳ１０１の処理に戻り、再び領域種を検索する。そして、線分群に含まれる領域種がなくなった場合（ステップＳ１０２：ＮＯ）、今まで得られている平面パラメータを出力して処理を終了する。
ここで、本変形例においては、領域種を検索し、３つの線分が同一平面に属するか否かの判定、及び領域拡張処理を行う際に基準平面又はこれを更新した更新平面に属するか否かの判定には、上記式（８）を使用する。すなわち、平面方程式の２乗平均誤差ｒｍｓが所定の閾値（th_rms）未満である場合にのみその線分（群）を同一平面に属するものと推定し、その線分を含めた平面として再び平面を算出する。このように平面方程式の２乗平均誤差ｒｍｓを使用して同一平面に属するか否かを判定することにより、更にノイズにロバストでかつ、細かい段差を含んでいるような場合にも正確に平面を抽出することができる。以下にその理由について説明する。
図５１は、その効果を示す図であって、端点と直線との距離が等しくても平面方程式の２乗平均誤差ｒｍｓが異なる例を示す模式図である。ここで、非特許文献４のように、領域拡張処理する際、注目の直線（線分）の端点（end point）と平面Ｐとの距離Ｄの値が所定の閾値より小さい場合に、当該注目の線分が平面Ｐと同一平面であるとして領域拡張処理を行うと、平面Ｐに交差する直線Ｌａ（図５１Ａ）と、平面Ｐと平行で所定距離ずれているような直線Ｌｂ（図５１Ｂ）とが同様に平面Ｐの更新に使用されることとなる。ここで、平面方程式の２乗平均誤差ｒｍｓを求めると、図５１Ａの直線Ｌａから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌａ）に比して図５１Ｂの直線Ｌｂから求まる平面方程式の２乗平均誤差ｒｍｓ（Ｌｂ）の方が大きい。すなわち、図５１Ａのように、直線Ｌａと平面Ｐとが交差する場合は、平面方程式の２乗平均誤差ｒｍｓが比較的小さくノイズの影響である場合が多いのに対し、図５１Ｂのような場合、平面方程式の２乗平均誤差ｒｍｓが大きく、直線Ｌｂは平面Ｐと同一平面ではなく異なる平面Ｐ’である確率が高い。したがって、複数の平面が含まれるような環境から平面を精確に求める必要がある場合などにおいては、本変形例のように、平面方程式の２乗平均誤差ｒｍｓを算出し、この値が所定の閾値未満である場合に同一平面と判断することが好ましい。なお、環境や距離データの性質に応じて、従来と同様、線分の端点と平面との距離が所定の閾値以下の場合は当該線分を平面に含めるようににたり、これらを組み合わせてもよい。
また、面パラメータｎ，ｄを一旦算出すれば、平面方程式の２乗平均誤差ｒｍｓは、データ点群について線分抽出の際に求めた２つのモーメントの値から平面方程式を更新し、上記式（８）にて簡単に算出することができる。
また、上述の領域種の選択方法は、図５２のようにも表現することができる。overlap(lj，lk)は、各イメージロウに含まれる直線ベクトルｌｊとｌｋにおける端点間の位置が直線ベクトルとは直交する位置にて重なっている場合にtrueを出力する関数である。また、fitPlane(l1，l2，l3)は、上記式（４）〜（７）によりＡｍ＝ｂの解を求め平面パラメータｎ，ｄを計算し、上記式（８）により算出されたＡ，ｂにより、直線ベクトルｌ１，ｌ２，ｌ３を平面にフィッティングさせる関数である。
rms(l1，l2，l3)は、上記式（６）を使用して３本の直線全てにおいて、平面方程式の２乗平均誤差ｒｍｓの値を算出する関数である。また、remove(l1，l2，l3)は、lines［i］，lines［i+1］l2，lines［i+2］から領域種を構成するとして選択されたそれぞれ直線l1，l2，l3を除くことを意味し、これにより、再びこれらの直線が計算に使用されることを防止する。
また、領域拡張処理は、図５３のように表現することもできる。図５３において、Ａ及びｂは、上記式（６−１）に示すそれぞれ行列及びベクトルである、また、add(A，b，l)は、上記式（８）により、Ａとｂに直線lineのモーメントを加える関数である。Solve(A，b)は、Ａｍ＝ｂを満たすｍを求め、上記式（４）〜（７）により平面パラメータｎ，ｄを計算する。select(open)は、例えば最初の１つなど、openの中から任意に１つのエレメントを選択する関数である。また、index(l1)は、画素列又は行におけるl1のインデックスを返す関数である。また、neighbor(index)は、与えられたインデックスに隣接したインデックス、例えば{index-1，index+1}を返す関数である。
また、上述したように、本変形例においては、図４３のステップＳ７３において領域拡張処理を行って平面方程式を更新した後、ステップＳ７４において平面方程式を再度算出する処理（Post processing）を行う。この再度算出する処理では、例えば上述のように更新され最終的に得られた平面方程式が示す平面に属するとされた距離データ点又は線分の平面からのずれを計算し、所定の値以上平面から外れる距離データ点又は線分は除き、再度平面方程式を更新することで、ノイズの影響を更に低減することができる。
次に、このステップＳ７４について詳細に説明する。ここでは、２つのステップにより、平面方程式を再度算出する方法について説明する。先ず、ステップＳ７３にて検出された各平面の境界の距離データ点（pixels）において、現在属している平面よりも、隣接する平面までの距離が近いデータ点が検出された場合は、当該データ点を隣接する平面の方に含める処理をする。また、何れの平面にも属していなく、かつ距離が例えば１．５ｃｍなど比較的大きい閾値以下である平面が存在するデータ点が検出できた場合は、当該データ点をその平面に含める処理をする。これらの処理は各平面領域の境界近傍のデータ点を検索することで実行することができる。以上の処理が終了したら、再度平面方程式を算出する。
次に、上述のようにして再度算出された平面の各領域の境界近傍において、各データ点と平面との距離が例えば０．７５ｃｍなど比較的小さい閾値を超える場合は、それらのデータ点を捨てる処理を実行する。これにより、その平面領域は若干小さくなるものの更に精確な平面を求めることができる。距離データ点を削除後、再び平面を求め、この処理を繰り返す。このことにより、極めて精密に平面を求めることができる。
次に各処理によって得られる結果を示す。図５４Ａは、ロボット装置が立った状態で床面を見下ろした際の床面を示す模式図、図５４Ｂは、縦軸をｘ、横軸をｙ、各データ点の濃淡でｚ軸を表現して３次元距離データを示す図であり、更に、行方向の画素列から線分抽出処理にて同一平面に存在するとされるデータ点群から直線を検出したものを示す。図５４Ｂに示す直線群から領域拡張処理によりえられた平面領域を図５４Ｃに示す。このように、ロボット装置の視野内には、１つの平面（床面）のみが存在する、すなわち、床面が全て同じ平面として検出されていることがわかる。
次に、床面に段差を一段置いたときの結果を図５５に示す。図５５Ａに示すように、床面Ｆには、１段の段差ＳＴ３が載置されている。図５５Ｂは、実験条件を示す図であり、着目点と直線（線分）との距離がmax_dを超える場合は、データ点群を分割する。また、抽出の成否（水平）（correct extraction(horizontal)）は、行方向のデータ列毎に、合計１０回の線分抽出を行う線分拡法による平面検出を行って成功した回数を示し、抽出の成否（垂直）（correct extraction(vertical)）は、列方向のデータ列毎についての抽出の成否を示す。また、Ｎｏ．１〜Ｎｏ．５は、上述したZig-Zag-Shape判別処理を取り入れていない従来の線分拡張法による平面検出処理の条件、Ｎｏ．６は、Zig-Zag-Shape判別処理を行った本変形例における平面検出方法の条件を示す。
図５５Ｃ及び図５５Ｄは、線分拡張法により平面検出した結果を示す図であって、それぞれ本変形例における手法により平面検出した結果、従来の線分拡張法により平面検出した結果（比較例）を示す。図５５Ｂに示すように、従来の手法においては、線分抽出（Line Fitting）において推定のための閾値パラメータmax_dを大きくする（max_d＝２５，３０）と検出率が下がり、閾値max_d小さくする(max_d＝１０，１５)と検出率が向上する。これに対して、本発明のように、ジグザグ形検証処理を導入することにより、大きな閾値max_d＝３０を設定しても、優れた検出結果を示すことがわかる。
すなわち、閾値max_dを大きくすると、ノイズの影響が少なくなるものの、線分抽出が難しくなり、閾値max_dを小さくすると、ノイズの影響を受けて誤検出が多くなってしまう。図５６Ｂに示す床面を撮影した画像から３次元距離データを取得した場合を図５６Ｂ及び図５６Ｃに示す。何れ左図は、行方向の画素列（距離データ列）から線分を抽出した例、右図は列方向の画素列（距離データ列）から線分を抽出した例を示す。図５６Ｂに示すように、閾値max_dを小さくすると、ノイズの影響が大きくなり、ノイズの影響が大きい遠方などにおいては特に、線分をうまく検出することができない。一方、図５６Ｃに示すように、従来の線分抽出に更にジグザグ形判別処理を加えた場合、閾値max_dを大きくしても、更にノイズの影響が大きい遠方の領域であっても線分が検出されていることがわかる。
これにより、上述した如く、それぞれ異なる階段を撮影した画像から３次元距離データを取得して平面検出することができる。例えば、図１１及び図１２に示すように、何れの場合も全ての踏面を平面として検出できている。なお、図１２Ｂでは、床面の一部も他の平面として検出成功している。
本変形例によれば、線分拡張法による平面検出を行う際、始めは大きな閾値を設定して線分を分割し、次にZig-Zag-Shape判別処理により、閾値を超えるデータ点を持たない直線であってもジグザグ形である場合には、ノイズではなく、複数平面からなる直線であるとして線分を分割するようにしたので、ノイズを含む距離情報から複数の平面を精度よく検出することが可能となる。
このように、小さい段差も精度よく検出することができるため、例えばロボット装置が移動可能な環境内の階段などを認識することができ、二足歩行ロボット装置であれば、この検出結果を利用して階段昇降動作が可能となる。
更に、複数の平面によって構成されている凸凹の床面を歩行可能な平面だと誤認識することがなくなり、ロボット装置の移動などが更に簡単になる。
なお、本発明は上述した変形例のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。また、上述した平面検出処理、階段認識処理、階段昇降制御処理のうち１以上の任意の処理は、ハードウェアで構成しても、演算器（ＣＰＵ）にコンピュータプログラムを実行させることで実現してもよい。コンピュータプログラムとする場合には、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。
【図１】
0004618247.tif 000010
【図２】
0004618247.tif 000011
【図３】
0004618247.tif 000012
【図４】
0004618247.tif 000013
【図５】
0004618247.tif 000014
【図６】
0004618247.tif 000015
【図７】
0004618247.tif 000016
【図８】
0004618247.tif 000017
【図９】
0004618247.tif 000018
【図１０】
0004618247.tif 000019
【図１１】
0004618247.tif 000020
【図１２】
0004618247.tif 000021
【図１３】
0004618247.tif 000022
【図１４】
0004618247.tif 000023
【図１５】
0004618247.tif 000024
【図１６】
0004618247.tif 000025
【図１７】
0004618247.tif 000026
【図１８】
0004618247.tif 000027
【図１９】
0004618247.tif 000028
【図２０】
0004618247.tif 000029
【図２１】
0004618247.tif 000030
【図２２】
0004618247.tif 000031
【図２３】
0004618247.tif 000032
【図２４】
0004618247.tif 000033
【図２５】
0004618247.tif 000034
【図２６】
0004618247.tif 000035
【図２７】
0004618247.tif 000036
【図２８】
0004618247.tif 000037
【図２９】
0004618247.tif 000038
【図３０】
0004618247.tif 000039
【図３１】
0004618247.tif 000040
【図３２】
0004618247.tif 000041
【図３３】
0004618247.tif 000042
【図３４】
0004618247.tif 000043
【図３５】
0004618247.tif 000044
【図３６】
0004618247.tif 000045
【図３７】
0004618247.tif 000046
【図３８】
0004618247.tif 000047
【図３９】
0004618247.tif 000048
【図４０】
0004618247.tif 000049
【図４１】
0004618247.tif 000050
【図４２】
0004618247.tif 000051
【図４３】
0004618247.tif 000052
【図４４】
0004618247.tif 000053
【図４５】
0004618247.tif 000054
【図４６】
0004618247.tif 000055
【図４７】
0004618247.tif 000056
【図４８】
0004618247.tif 000057
【図４９】
0004618247.tif 000058
【図５０】
0004618247.tif 000059
【図５１】
0004618247.tif 000060
【図５２】
0004618247.tif 000061
【図５３】
0004618247.tif 000062
【図５４】
0004618247.tif 000063
【図５５】
0004618247.tif 000064
【図５６】
0004618247.tif 000065