(57)【要約】【課題】オドメトリによる誤差の影響を低減させ、データ抜けの少ない密な環境地図を生成することができる環境地図生成方法、及び移動ロボットを提供すること。【解決手段】本発明にかかる環境地図生成方法は、移動ロボット１００が参照する環境地図を生成する環境地図生成方法であって、現フレームより平面情報を検出するステップと、移動ロボット１００の移動量に基づいて、現フレームの前に取得された前フレームの座標系を現フレームの座標系に変換するステップと、座標変換された前フレームを現フレームに重ね合わせるステップと、検出された平面情報と、重ね合わされた現フレームとに基づいて環境地図を生成するステップとを有するものである。【選択図】図２
2008276348.tif 000002
【特許請求の範囲】
【請求項１】
移動ロボットが参照する環境地図を生成する環境地図生成方法であって、
現フレームより平面情報を検出するステップと、
前記移動ロボットの移動量に基づいて、前記現フレームの前に取得された前フレームの座標系を前記現フレームの座標系に変換するステップと、
座標変換された前記前フレームを前記現フレームに重ね合わせるステップと、
検出された前記平面情報と、重ね合わされた前記現フレームとに基づいて前記環境地図を生成するステップとを
有する環境地図生成方法。
【請求項２】
前記重ね合わせステップは、
座標変換された前記前フレームにおける平面について、前記現フレームにおいて対応する平面を特定し、座標変換された前記前フレームにおける平面の３次元位置データの高さ成分を、前記現フレームにおいて特定された前記平面の３次元位置データの高さ成分により置き換えることにより、重ね合わせ処理を実行する
ことを特徴とする請求項１記載の環境地図生成方法。
【請求項３】
前記平面情報の検出ステップは、
３次元位置データ群から２点の３次元位置データの組をサンプリングし、該２点の３次元位置データの組に基づいて平面パラメータを算出するステップと、
算出された平面パラメータからなる平面に対して、３次元位置データが属する平面を決定するステップと
を備えることを特徴とする請求項１記載の環境地図生成方法。
【請求項４】
前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行う
ことを特徴とする請求項１乃至３記載の環境地図生成方法。
【請求項５】
環境地図を参照して移動する移動ロボットであって、
前記環境を視覚的に認識する視覚センサと、
前記視覚センサにより得られる計測データを用いて、前記環境に存在する計測対象の位置を示す３次元位置データ群を生成する３次元位置データ生成部と、
現フレームの３次元位置データ群より平面情報を検出する平面検出部と、
前記移動ロボットの移動量に基づいて、前記現フレームの前に取得された前フレームの座標系を前記現フレームの座標系に変換する座標変換部と、
座標変換された前記前フレームを前記現フレームに重ね合わせる重ね合わせ部と、
検出された前記平面情報と、重ね合わされた前記現フレームとに基づいて前記環境地図を生成する環境地図生成部と
を備える移動ロボット。
【請求項６】
前記重ね合わせ部は、
座標変換された前記前フレームにおける平面について、前記現フレームにおいて対応する平面を特定し、座標変換された前記前フレームにおける平面の３次元位置データの高さ成分を、前記現フレームにおいて特定された前記平面の３次元位置データの高さ成分により置き換えることにより、重ね合わせ処理を実行する
ことを特徴とする請求項５記載の移動ロボット。
【請求項７】
前記平面検出部は、
３次元位置データ群から２点の３次元位置データの組をサンプリングし、該２点の３次元位置データの組に基づいて平面パラメータを算出する平面パラメータ算出部と、
算出された平面パラメータからなる平面に対して、３次元位置データが属する平面を決定するラベリング処理部と
を備えることを特徴とする請求項５記載の移動ロボット。
【請求項８】
前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行う
ことを特徴とする請求項５乃至７記載の移動ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は環境地図生成方法、及び移動ロボットに関する。
【背景技術】
【０００２】
自律移動ロボットは、自ら移動経路の環境を認識して、目的地へと移動する。このため、移動ロボットは、ステレオカメラやレンジセンサなどの視覚センサによって得られる計測データを用いて環境地図を生成し、その環境地図を参照して目的地へと移動する。
自律移動ロボットが２足歩行ロボットである場合には、環境地図を用いて足平を着地可能な領域を判断し、足平の着地位置を順次計画することによって歩行を計画する。
従って、移動ロボットの安定した歩行を計画するためには、データ抜けの少ない密な環境地図を生成することが必要である。
【０００３】
環境地図は、視覚センサによって得られた視差画像や、計測対象との距離画像などの計測情報をもとに算出される３次元位置データ群から平面群を検出することによって生成することができる。３次元位置データ群から平面を検出する方法としては、ハフ変換を用いた平面検出手法や、ランダムサンプリングによる平面検出手法などが良く知られている。例えば特許文献１には、ハフ変換を利用して３次元位置データ群から平面パラメータを算出することによって、２足歩行ロボットの足平が接している床面を含む複数平面を検出し、これら複数平面の検出結果を用いて障害物領域と移動可能領域とが特定された環境地図を生成する技術が開示されている。
また、特許文献２には、外部状態から、３次元障害物地図及び２次元の基準面からの高さを示す対基準面高さ地図を作成し、３次元障害物地図に基づいて対基準面高さ地図を変更する環境地図生成方法が開示されている。
【特許文献１】特開２００５−９２８２０号公報
【特許文献２】特開２００６−１１８８０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、従来知られているいずれの環境地図生成方法によっても、計測データの分解能が低い場合や、視覚センサとの通信エラーなどによるデータ抜け・データ不足が発生した場合などにおいては、データ抜けの少ない密な環境地図を得ることはできないものであった。例えば、環境地図にデータ抜けがある場合に、本来は着地可能な領域に対して着地することができないときには、最適な経路以外の他の経路を生成しなければならず、不要な方向転換などが発生するという問題がある。
一方、このようなデータ抜けを補完する手法としては、計測データがプロットされた現フレームに対して、現フレームより前に取得された前フレームを重ね合わせることによりデータ抜けを補完する手法が考えられる。前フレームを現フレームへと重ね合わせる際には、前フレームをオドメトリによって座標変換することにより現フレームへと対応させる。しかし、このような対応付けにより重ね合わせを実行した場合には、オドメトリの誤差によって、座標変換後の前フレーム及び現フレームのデータについて整合性がとれなくなり、このような現フレーム及び前フレームを重ねあわせたフレームからでは、精度の高い平面を検出することができない。
特に、移動ロボットが２足歩行ロボットである場合には、高さ、ピッチ、及びロール方向への変動が大きいため、オドメトリの誤差によって高さ方向の精度が失われてしまう。このため、例えば、移動ロボットが段差などの平面に足平を着地する際に特に重要となる、平面の高さ成分を精度良く検出することができない。
【０００５】
このように、現フレーム及び前フレームを重ねあわせたフレームから検出された平面を環境地図生成のために使用しては不十分なものであると共に、データ抜け・データ不足を含む環境地図を参照しては、移動ロボットの動作計画を安定させることができないという問題がある。
本発明は、かかる課題を解決するためになされたものであり、オドメトリによる誤差の影響を低減させ、データ抜けの少ない密な環境地図を生成することができる環境地図生成方法、及び移動ロボットを提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明にかかる環境地図生成方法は、移動ロボットが参照する環境地図を生成する環境地図生成方法であって、現フレームより平面情報を検出するステップと、前記移動ロボットの移動量に基づいて、前記現フレームの前に取得された前フレームの座標系を前記現フレームの座標系に変換するステップと、座標変換された前記前フレームを前記現フレームに重ね合わせるステップと、検出された前記平面情報と、重ね合わされた前記現フレームとに基づいて前記環境地図を生成するステップとを有するものである。
【０００７】
これにより、現フレームの３次元位置データ群のデータ抜けを前フレームの３次元位置データ群によって補完することができ、データ抜けの少ない密な環境地図を生成することができる。また、現フレームのみから平面を検出するため、現フレーム及び前フレームを重ね合わされたフレームから平面を検出する場合と比較して、オドメトリによる誤差の影響を受けずに精度の高い平面を検出することができる。
従って、このようにして検出された平面情報と、重ね合わされた現フレームの３次元位置データ群とによれば、データ抜けの少ない密な環境地図を生成することができ、移動ロボットの動作計画の解を安定させることができる。
【０００８】
また、前記重ね合わせステップは、座標変換された前記前フレームにおける平面について、前記現フレームにおいて対応する平面を特定し、座標変換された前記前フレームにおける平面の３次元位置データの高さ成分を、前記現フレームにおいて特定された前記平面の３次元位置データの高さ成分により置き換えることにより、重ね合わせ処理を実行するようにしてもよい。これにより、現フレームの特定された平面の高さを基準高さとして、前フレームの３次元位置データを置き換えることができるため、オドメトリによる誤差の影響を受けて、置き換えによって３次元位置データの整合性が崩れてしまう場合であっても、高さ方向のズレを補正することができる。
【０００９】
さらにまた、前記平面情報の検出ステップは、３次元位置データ群から２点の３次元位置データの組をサンプリングし、該２点の３次元位置データの組に基づいて平面パラメータを算出するステップと、算出された平面パラメータからなる平面に対して、３次元位置データが属する平面を決定するステップとを備えるようにしてもよい。これにより、ハフ変換を利用する手法と比較して、正確かつ安定に平面を抽出することができる。例えば、環境に階段などの複数の平面が存在する場合には、ハフ変換による手法に比べて、より多くの平面を安定して正確に検出することができる。
【００１０】
また、前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行うようにしてもよい。移動ロボットが二足歩行ロボットである場合には、オドメトリによる誤差によって高さ方向の精度が失われてしまうものの、かかる場合であっても、高さ方向の誤差を補正することができる。
【００１１】
本発明にかかる移動ロボットは、環境地図を参照して移動する移動ロボットであって、前記環境を視覚的に認識する視覚センサと、前記視覚センサにより得られる計測データを用いて、前記環境に存在する計測対象の位置を示す３次元位置データ群を生成する３次元位置データ生成部と、現フレームの３次元位置データ群より平面情報を検出する平面検出部と、前記移動ロボットの移動量に基づいて、前記現フレームの前に取得された前フレームの座標系を前記現フレームの座標系に変換する座標変換部と、座標変換された前記前フレームを前記現フレームに重ね合わせる重ね合わせ部と、検出された前記平面情報と、重ね合わされた前記現フレームとに基づいて前記環境地図を生成する環境地図生成部とを備えるものである。
【００１２】
これにより、現フレームのデータ抜けを前フレームによって補完することができ、データ抜けの少ない密な環境地図を生成することができる。また、現フレームのみから平面を検出するため、現フレーム及び前フレームを重ね合わされたフレームから平面を検出する場合と比較して、オドメトリによる誤差の影響を受けずに精度の高い平面を検出することができる。
従って、このようにして検出された平面情報と、重ね合わされた現フレームの３次元位置データ群とによれば、データ抜けの少ない密な環境地図を生成することができ、移動ロボットの動作計画の解を安定させることができる。
【００１３】
また、前記重ね合わせ部は、座標変換された前記前フレームにおける平面について、前記現フレームにおいて対応する平面を特定し、座標変換された前記前フレームにおける平面の３次元位置データの高さ成分を、前記現フレームにおいて特定された前記平面の３次元位置データの高さ成分により置き換えることにより、重ね合わせ処理を実行するようにしてもよい。これにより、現フレームの特定された平面の高さを基準高さとして、前フレームの３次元位置データを置き換えることができるため、オドメトリによる誤差の影響を受けて、置き換えによって３次元位置データの整合性が崩れてしまう場合であっても、高さ方向のズレを補正することができる。
【００１４】
さらにまた、前記平面検出部は、３次元位置データ群から２点の３次元位置データの組をサンプリングし、該２点の３次元位置データの組に基づいて平面パラメータを算出する平面パラメータ算出部と、算出された平面パラメータからなる平面に対して、３次元位置データが属する平面を決定するラベリング処理部とを備えるようにしてもよい。これにより、ハフ変換を利用する手法と比較して、正確かつ安定に平面を抽出することができる。例えば、環境に階段などの複数の平面が存在する場合には、ハフ変換による手法に比べて、より多くの平面を安定して正確に検出することができる。
【００１５】
また、前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行うようにしてもよい。移動ロボットが二足歩行ロボットである場合には、オドメトリによる誤差によって高さ方向の精度が失われてしまうものの、かかる場合であっても、高さ方向の誤差を補正することができる。
【発明の効果】
【００１６】
本発明によれば、オドメトリによる誤差の影響を低減させ、データ抜けの少ない密な環境地図を生成することができる環境地図生成方法、及び移動ロボットを提供することを目的とする。
【発明を実施するための最良の形態】
【００１７】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。尚、各図面において、同一要素には同一の符号を付しており、説明の明確化のため、必要に応じて重複説明を省略する。
【００１８】
発明の実施の形態１．
本実施の形態にかかる移動ロボットは、２つの脚リンクを有する脚式移動ロボットである。さらに、移動ロボットは環境地図生成装置を有する。環境地図生成装置は、現フレームより平面情報を検出し、移動ロボットの移動量に基づいて、現フレームの前に取得された前フレームの座標系を現フレームの座標系に変換する。次いで、座標変換された前フレームの３次元位置データ群を現フレームの３次元位置データ群に重ね合わせ、検出された平面情報と、重ね合わされた現フレームの３次元位置データ群とに基づいて環境地図を生成する装置である。
移動ロボットは、環境地図生成装置によって生成された環境地図を参照して、移動ロボットの２つの脚リンクそれぞれの先端に設けられた足平の着地位置を決定する。
さらに、移動ロボットは、決定された足平着地位置を実現するための動作データを生成し、生成された動作データをもとに移動ロボットの関節群をアクチュエータによって駆動することで歩行する。
【００１９】
図１を用いて、本発明にかかる移動ロボット１００の構成について説明する。図１は、移動ロボット１００を側方（移動ロボット１００の移動する方向を向いて左方向）から見た様子を概略的に表す概略図であり、移動ロボット１００に搭乗者Ｐが搭乗している様子を表している。図１に示すように、視覚センサとしてのレーザレンジファインダ１０が移動ロボット１００に取り付けられる。
尚、図１においては、説明の便宜上、移動体１が進行する向き（前後方向）をｘ軸、移動体１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行方法、ｚ軸は紙面中の上下方向を示す。
【００２０】
図１に示すように、移動ロボット１００は、搭乗者Ｐが搭乗する載置台（図示せず）を含む上体部１０１と、上体部１０１を支持する腰部（図示せず）を介して回動自在に固定される脚部１０４と、腰部に取り付けられたレーザレンジファインダ１０とにより構成される。
脚部１０４は、右脚１０２及び左脚１０３により構成され、右脚１０２は、右膝関節１０２ｃ、右足首関節１０２ｅ、右足平１０２ｆを備え、同様に、左脚１０３は、左膝関節１０３ｃ、左足首関節１０３ｅ、左足平１０３ｆを備える。
図示しないモータからの駆動力が、同じく図示しないプーリ及びベルトを介して伝達されることで、各関節が所望の角度に駆動され、その結果、右脚１０２及び左脚１０３に所望の動きをさせることができる。
【００２１】
移動ロボット１００の上体部１０１には、腰部（図示せず）を介して移動ロボット１００が歩行する環境の距離画像データを検出するレーザレンジファインダ１０が設けられている。より具体的には、例えば図１においては、レーザレンジファインダ１０を、水平方向から移動ロボット１００の足元に向けて傾斜させて設置し、移動ロボット１００の足元近傍を含む前方に向けて赤外線などのレーザ光を照射し、その反射光を受光させる。
このようにすると、移動ロボット１００の足元近傍の距離画像データについて高い分解能を確保することができ、密度の高い足元近傍の距離画像データを取得することができる。
【００２２】
レーザレンジファインダ１０は、床面上の障害物を含む外部環境の距離画像データを取得する。尚、距離画像データとは、環境に存在する計測対象までの距離値を各画素の画素値として有する２次元データである。後述するように、距離画像データの座標変換を行うことにより、外界の３次元の点群データを生成することができる。
【００２３】
さらに、レーザレンジファインダ１０は、移動ロボット１００の腰部に備えられており、鉛直軸を中心に揺動可能な構成となっている。これにより、移動ロボット１００の移動方向前方をスキャンすることができる。
このようにすると、上面視では、レーザ光によるスキャン範囲を移動ロボット１００の移動方向前方に向かって、左右に広がった扇形形状となるように構成することができ、移動に必要な領域のみを効率的にスキャンすることができる。
【００２４】
ここで、レーザレンジファインダ１０が、２次元の距離画像データから３次元の点群データを生成する方法について説明する。
まず、レーザレンジファインダ１０を左右に揺動せずに固定した場合に、レーザ光を床面に垂直に移動方向前方に向かって上下に照射することで、床面に垂直な平面上における障害物の距離画像データを取得する。そして、レーザレンジファインダ１０を移動ロボット１００の鉛直軸を中心に、移動方向前方に向かって左右に揺動しながらスキャンすることで、複数の異なる平面上における距離画像データを取得する。次いで、レーザレンジファインダ１０を揺動可能に支持する関節の角度に基づいて複数の距離画像データを座標変換して位置合わせする。これにより、複数平面上で得られた距離画像データの統合を行うことができる。または、複数の距離画像データから対応点を求めることによって、距離画像データ相互の位置合わせを行ってもよい。
このようにして、前方の領域に含まれる３次元の距離画像データを簡便な構成によって容易に取得することができる。
【００２５】
続いて以下では、本実施の形態にかかる移動ロボット１００の制御系について説明する。移動ロボット１００の制御系の構成を図２に示す。図２に示すように、環境地図生成装置１は、平面検出部１２、座標変換部１３、重ね合わせ部１４、及び環境地図生成部１５を有する。
【００２６】
図２において、レーザレンジファインダ１０は、上述したように、移動ロボット１００の外部環境の距離画像データを取得する。より具体的には、例えば図３に示すように、移動ロボット１００が移動方向（ここではｘ軸方向）へ移動する際に、レーザレンジファインダ１０より移動方向前方に向けてレーザ光を照射することにより、移動方向前方の距離画像データを検出する。
尚、視覚センサとして、レーザレンジファインダに代えてステレオカメラを用いてもよい。つまり、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備え、これら複数のカメラによって撮影された画像の視差を利用して距離画像データを生成してもよい。より具体的には、複数のカメラによって撮影した画像データから対応点を検出し、ステレオ視によって対応点の３次元位置を復元する。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。
【００２７】
３次元位置データ生成部１１は、距離画像データの座標変換を行って３次元位置データ群を生成する。尚、３次元位置データ群とは、距離画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。
例えば図４の画像は、移動ロボット１００により生成される３次元位置データ群の一例を示している。尚、図４は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。また、図４において破線で囲まれた領域１６２は、センサとの通信エラーなどにより発生したデータ抜け領域を表している。
ここで、図４の画像をフレームと呼ぶ。移動ロボット１００により処理対象となる現在のフレームを現フレームと呼び、現フレームの以前に生成されたフレームを前フレームと呼ぶ。移動ロボット１００が移動することで、前フレームと現フレームとでは３次元位置データ群が変化する。
【００２８】
また、３次元位置データ生成部１１は、オドメトリを用いて、生成した３次元位置データの歪みを補正する。より具体的には、レーザレンジセンサ１０によるスキャン中に、移動ロボット１００が移動することによって発生する検出誤差を補正する。即ち、レーザレンジセンサ１０によるスキャン中に移動ロボット１００が移動するため、検出される床面形状データは、移動ロボット１００の移動量に応じた検出誤差を含む。オドメトリにより、各関節軸に設けられたエンコーダ２０などの出力に基づいて移動ロボット１００の移動量を算出して、移動ロボット１００が移動することによる検出誤差を補正する。
さらにまた、３次元位置データ生成部１１は、移動ロボット１００の上体部１０１に設けられたジャイロ（図示せず）からの出力に基づいて、３次元位置データ群のピッチ方向の歪みを補正する。即ち、移動ロボット１００の腰部に設けられたジャイロ（図示せず）の出力からピッチ方向の回転成分を検出し、３次元位置データのピッチ方向の検出誤差を補正する。これにより、歩行ロボットにおいて３次元位置データに与える影響が大きいピッチ方向の検出誤差を補正する。
【００２９】
平面検出部１２は、３次元位置データ生成部１１により生成された３次元位置データ群より平面を検出する。より具体的には、平面検出部１２は、３次元位置データ群から２点の３次元位置データの組をサンプリングし、その２点の３次元位置データの組に基づいて平面パラメータを算出する平面パラメータ算出部と、算出された平面パラメータからなる平面に対して、３次元位置データが属する平面を決定するラベリング処理部とを備える。
このような２点ランダムサンプリング法は、ハフ変換を利用する手法と比較して、正確かつ安定に平面を抽出することができる。例えば、環境に階段などの複数の平面が存在する場合には、ハフ変換による手法に比べて、より多くの平面を安定して正確に検出することができる。
尚、平面検出方法はこれに限定されず、従来知られているハフ変換を利用する手法を用いて平面を検出してもよい。
【００３０】
座標変換部１３は、移動ロボット１００の移動量に基づいて、前フレームの座標系を現フレームの座標系に変換する。本実施の形態においては、現フレームの直前に生成されたフレームを前フレームとして利用する。
オドメトリによる誤差は累積してゆくため、現フレームに対してより以前に生成されたフレームほどオドメトリによる誤差の影響を大きく受ける。このため、現フレームに対してオドメトリによる誤差が相対的に少ない直前のフレームを重ね合わせに利用することが好ましい。
【００３１】
重ね合わせ部１４は、座標変換された前フレームを現フレームに重ね合わせる。さらに、重ね合わせ部１４は、座標変換された前フレームにおける平面について、現フレームにおいて対応する平面を特定し、座標変換された前フレームにおける平面の３次元位置データの高さ成分を、現フレームにおいて特定された平面の３次元位置データの高さ成分により置き換えることにより、重ね合わせ処理を実行する。
尚、重ね合わせ部１４による現フレームへの重ね合わせ処理の詳細は後述する。
【００３２】
環境地図生成部１５は、平面検出部１２により検出された平面情報と、重ね合わせ部１４により重ね合わされた現フレームとに基づいて、複数の平面の集合としての環境地図を生成する。
より具体的には、まず、平面検出部１２による平面検出結果をもとに、床面領域を移動ロボット１００が歩行可能な移動可能領域として認識し、床面とは別の平面が含まれる領域を障害物領域として認識する。
尚、床面とは、移動ロボット１００の右足平１０２ｆ及び左足平１０３ｆが接している平面を意味する。ただし、このような領域認識は一例である。例えば、床面とは高さが異なる平面や斜面であっても、床面との高さの差が所定の値より小さい場合に右足平１０２ｆ及び左足平１０３ｆを着地させることが可能であれば、これらの平面を移動可能領域として認識してもよい。さらにまた、検出された平面の面積と右足平１０２ｆ及び左足平１０３ｆの面積とを比較した結果によって、その平面を移動可能領域として認識するかを決定してもよい。
次いで、環境地図更新生成部１５は、認識された移動可能領域及び障害物領域と、重ね合わせ部１４により重ね合わされた現フレームの３次元位置データ群とを統合して環境地図を生成する。
【００３３】
ここで、本実施の形態における環境地図は、２次元平面（ｘｙ平面とする）を格子状に分割したグリッドデータとして生成される。環境地図の各グリッドに対応するデータとして、各グリッドのｘｙ平面に垂直なｚ軸方向の高さ、各グリッドが属する平面を一意に識別可能な平面ＩＤ、及び、各グリッドが属する平面の法線ベクトルが保持される。環境地図生成部１５は、ほぼ同じ高さの隣接するグリッドの集合を１つの平面と認識し、認識した１つの平面に対して一意な平面ＩＤを付与する。
【００３４】
例えば、図５（ａ）に示す環境１６０を移動ロボット１００が歩行する場合、床面に相当する平面Ｐ１１及び障害物１６１の上面に相当する平面Ｐ１２が平面検出部１２により検出される。そして、環境地図生成部１５によって、平面Ｐ１１が移動可能領域として認識され、平面Ｐ１２が障害物領域として認識される。このとき、環境地図生成部１５によって生成される環境地図２００は、図５（ｂ）に示すようになる。
図５（ｂ）の環境地図２００は、各グリッドのデータとして、各平面のそれぞれを一意に識別可能な平面ＩＤ、各グリッドのｚ軸方向の高さ、及び法線ベクトル（ｎａ，ｎｂ，ｎｃ）が保持される。
尚、隣接するグリッド間の平面ＩＤの違い、及びｚ軸方向の高さの違いによって右足平１０２ｆ及び左足平１０３ｆの着地位置における路面の凹凸を検出するため、環境地図２００を構成する各グリッドの面積は、右足平１０２ｆ及び左足平１０３ｆの底面の面積より十分に小さくなるよう選択するとよい。
【００３５】
さらに、図５（ｂ）の環境地図２００は、各グリッドのデータとして領域種別を示す情報、つまり移動可能領域であるか障害物領域であるかを示す識別情報が保持される。図５（ｂ）の例では、床面Ｐ１１に対応する領域Ｒ１１が移動可能領域であり、障害物１６１に対応する領域Ｒ１２（図５（ｂ）の斜線領域）が障害物領域である。
【００３６】
歩行計画部１６は、環境地図生成装置１によって生成された環境地図を参照して目標位置を決定し、決定した目標位置に到達するための右足平１０２ｆ及び左足平１０３ｆの着地位置を算出する。
【００３７】
動作生成部１７は、歩行計画部１６によって生成された右足平１０２ｆ及び左足平１０３ｆの着地位置を実現するための動作データを生成する。ここで、動作データとは、移動ロボット１００のＺＭＰ位置、重心位置、右足平１０２ｆ及び左足平１０３ｆの位置及び姿勢、並びに、移動ロボット１００の上体部１０１の位置及び姿勢を含む。
【００３８】
制御部１８は、動作生成部１７によって生成された動作データを入力し、逆運動学演算によって各関節の目標関節角度を算出する。さらに、制御部１８は、算出した目標関節角度とエンコーダ２０によって計測した現在の関節角度をもとに各関節を駆動するためのトルク制御値を算出する。制御部１８によって算出されたトルク制御値に従って各関節を駆動するためのアクチュエータ１９を動作させることにより、移動ロボット１００の歩行が行われる。
【００３９】
続いて以下では、環境地図生成装置１による環境地図生成方法の詳細を図６乃至９を用いて説明する。図６は、環境地図生成装置１による環境地図生成手順を示すフローチャートである。図７は、前フレーム及び現フレームの重ね合わせ処理を説明するための図である。図８は、前フレームの平面に対応する現フレームの平面を特定する処理を説明するための図である。図９は、前フレームにおける３次元位置データの高さ成分を、現フレームにおける３次元位置データの高さ成分として置き換えるようすを説明するための概念図である。尚、図７及び８は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。
【００４０】
まず、ステップＳ１０１では、平面検出部１２が、現フレームにおける３次元位置データ群より平面を検出する。即ち、平面情報としての複数の平面についての平面パラメータが算出され、現フレームにおける各３次元位置データが属する平面が決定される。
前フレーム及び現フレームを重ね合わせたフレームから平面を検出する場合には、平面検出に要する処理時間が従来の２倍程度となるため、移動ロボット１００の高速移動についての制約となる。このため、現フレームのみから平面を検出することにより、平面検出処理に要する処理時間を増加させずに、従来同様の処理時間を維持することができる。
また、前フレーム及び現フレームを重ね合わせたフレームから平面を検出する場合には、オドメトリによる誤差のため、重ね合わせ後のフレームにおける３次元位置データ群の整合性がとれず、精度の高い平面を検出することができない。このため、現フレームのみから平面を検出することがより好ましい。
尚、現フレームにはデータ抜けが含まれているものの、平面検出処理に際して問題となる程度ではなく、所望の精度の平面を検出することができる。
【００４１】
ステップＳ１０２では、座標変換部１３が、移動ロボット１００の移動量に基づいて、前フレームの座標系を現フレームの座標系に変換する。ここで、現フレームをＭｉ、前フレームをＭｉ−１と表した場合に、前フレームＭｉ−１の座標変換Ｆ（Ｍｉ−１）は以下の（１）式により一般的に表される。（１）式において、Ｒは回転行列であり、ｔは平行移動ベクトルである。回転行列Ｒ及び平行移動ベクトルｔは、エンコーダ２０などの計測情報より得られる移動ロボット１００の移動量及び姿勢変化を反映して決定される。
Ｆ（Ｍｉ−１）＝ＲＭｉ−１＋ｔ      （１）
【００４２】
ステップＳ１０３では、重ね合わせ部１４が、座標変換された前フレームを現フレームに重ね合わせる。より具体的には、座標変換された前フレームの３次元位置データを、現フレームに重ね合わせる。
図７（ａ）の画像は現フレームを表し、図７（ｂ）の画像は現フレームの前に生成された前フレームを表している。図７（ｂ）に示した移動ロボット１００が前方（白抜矢印Ｄ方向）に移動することで、前フレームと現フレームとでは３次元位置データ群が変化し、データ抜けが発生した領域も変化する。
図７（ｃ）の画像は、図７（ｂ）に示した前フレームを座標変換して、図７（ａ）に示した現フレームと重ね合わせた後のフレームを示している。
【００４３】
ステップＳ１０４では、重ね合わせ部１４が、座標変換された前フレームにおける平面について、現フレームにおいて対応する平面を特定する。例えば、平面のパラメータに基づいて、前フレームにおける平面Ａｉに対応する現フレームにおける平面Ａｊを特定する。平面パラメータとして、平面高さをｈ、傾きをφ及びθとして表し、平面パラメータを比較するための閾値をそれぞれＴＨｈ、ＴＨφ、及びＴＨθとする。前フレームにおける平面Ａｉに対して、以下の（２）〜（４）式の全てを満足する現フレーム平面Ａｊを特定する。
｜Ａｊ（ｈ）−Ａｉ（ｈ）｜＜ＴＨｈ      （２）
｜Ａｊ（φ）−Ａｉ（φ）｜＜ＴＨφ      （３）
｜Ａｊ（θ）−Ａｉ（θ）｜＜ＴＨθ      （４）
このように、平面パラメータを用いて対応する平面を特定することにより、速やかに対応する平面を特定することができる。
例えば図８（ａ）に示す前フレームの平面Ａｉ（図において破線で囲まれた領域Ａｉ）に対して、図８（ｂ）に示す現フレームにおける平面Ａｊ（図において破線で囲まれた領域Ａｊ）が特定される。
【００４４】
ステップＳ１０５では、座標変換された前フレームにおける平面の３次元位置データの高さ成分を、現フレームにおいて特定された平面の３次元位置データの高さ成分により置き換える。即ち、前フレームにおける平面Ａｉに属する３次元位置データ（Ａｉｘ，Ａｉｙ，Ａｉｚ）について、その高さ成分Ａｉｚを現フレームにおける平面Ａｊの高さ成分Ａｊｚにより置き換える。
図９（ａ）の画像は、座標変換した前フレームを現フレームに重ね合わせたようすを、高さ方向（図においてはｚ軸方向）について示す概念図である。図９（ａ）に示すように、オドメトリによる誤差のため、前フレームにおける平面Ａｉに属する３次元位置データは、現フレームにおける平面Ａｊに属する３次元位置データに対して整合性が崩れたものとなる。
一方、図９（ｂ）の画像は、座標変換した前フレームを現フレームに重ね合わせ、前フレームの平面Ａｉに対応する現フレームの平面Ａｊを特定した上で、３次元位置データの高さ成分を置き換えたようす示す概念図である。図９（ｂ）に示すように、前フレームの平面Ａｉに属する３次元位置データの高さ成分を、現フレームの平面Ａｊに属する３次元位置データの高さ成分により置き換えることにより、重ね合わせたフレームにおける平面の高さ成分が水平となり整合性がとれたものとなる。
例えば、図７（ｄ）の画像は、図７（ｃ）に示した重ね合わせ後のフレームにおいて、平面の高さ成分を現フレームの高さ成分により置き換えたフレームを示している。
これにより、現フレームの特定された平面の高さを基準高さとして、前フレームの３次元位置データを置き換えることができるため、オドメトリによる誤差のため、置き換えによって３次元位置データの整合性が崩れてしまう場合であっても、高さ方向のズレを補正することができる。
【００４５】
ステップＳ１０６では、環境地図生成部１５が、平面検出部１２により検出された平面情報と、重ね合わせ部１４により重ね合わされた現フレームとに基づいて、環境地図を生成する。
【００４６】
尚、上述した３次元位置データ生成部１１、平面検出部１２、座標変換部１３、重ね合わせ部１４及び環境地図生成部１５により実行される処理は、典型的なコンピュータシステムを用いて実現可能である。具体的には、一定の時間間隔で発生するタイマ割り込みに応じて、図６のフローチャートに示した処理をコンピュータシステムに行わせるためのプログラムをコンピュータシステムに実行させればよい。なお、図６のフローチャートに示した処理をコンピュータシステムに実行させるためのプログラムは、１のプログラムである必要はなく、処理内容により分割された複数のプログラムモジュールによって構成されてもよい。
【００４７】
以上に説明したように、本実施の形態にかかる移動ロボット１００は、現フレームの３次元位置データ群のデータ抜けを前フレームの３次元位置データ群によって補完することができ、データ抜けの少ない密な環境地図を生成することができる。また、現フレームのみから平面を検出するため、現フレーム及び前フレームを重ね合わされたフレームから平面を検出する場合と比較して、オドメトリによる誤差の影響を受けずに精度の高い平面を検出することができる。従って、このようにして検出された平面情報と、重ね合わされた現フレームの３次元位置データ群とによれば、データ抜けの少ない密な環境地図を生成することができ、移動ロボット１００の動作計画の解を安定させることができる。
また、現フレームのみから得られる環境地図に比べて、前フレームを用いることによって認識範囲が拡大されるため、例えば現フレームのみからでは認識できない移動ロボット１００の後方についても環境地図に反映することができ、より安定した歩行計画を立てることができる。
【００４８】
その他の実施の形態．
発明の実施の形態１は、脚式移動ロボットに本発明を適用した一例を示したが、車輪等のその他の移動手段を有する移動ロボットに対しても本発明を適用可能である。
【００４９】
さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【００５０】
【図１】本発明の実施の形態にかかる移動ロボットの構成を示す側面図である。
【図２】本発明の実施の形態にかかる移動ロボットが有する制御系のブロック構成図である。
【図３】本発明の実施の形態にかかる移動ロボットが移動する際にレーザレンジファインダによるスキャンの様子を説明するための図である。
【図４】本発明の実施の形態にかかる３次元位置データ群の一例を示す図である。
【図５】本発明の実施の形態にかかる移動ロボットが移動する環境の一例及び環境地図の一例を示す図である。
【図６】本発明の実施の形態にかかる環境地図生成装置の処理内容を示すフローチャートである。
【図７】本発明の実施の形態にかかる重ね合わせ処理を説明するための図である。
【図８】本発明の実施の形態にかかる平面を特定する処理を説明するための図である。
【図９】本発明の実施の形態にかかる高さ成分を置き換えるようすを説明するための概念図である。
【符号の説明】
【００５１】
１  環境地図生成装置、
１０  レーザレンジファインダ、１１  ３次元位置データ生成部、
１２  平面検出部、１３  座標変換部、１４  重ね合わせ部、
１５  環境地図生成部、
１６  歩行計画部、１７  動作生成部、
１８  制御部、１９  アクチュエータ、２０  エンコーダ
１００  移動ロボット、１０４  脚部、
１０２  右脚、１０２ｃ  右膝関節、１０２ｅ  右足首関節、１０２ｆ  右足平、
１０３  左脚、１０３ｃ  左膝関節、１０３ｅ  左足首関節、１０３ｆ  左足平、
１６０  環境、１６１  障害物、１６２  データ抜け領域、
Ｐ１１、１２  床面、２００  環境地図、
Ｐ  搭乗者、Ｒ１１、１２  領域
【図１】
2008276348.tif 000003
【図２】
2008276348.tif 000004
【図３】
2008276348.tif 000005
【図４】
2008276348.tif 000006
【図５】
2008276348.tif 000007
【図６】
2008276348.tif 000008
【図７】
2008276348.tif 000009
【図８】
2008276348.tif 000010
【図９】
2008276348.tif 000011