３次元環境復元装置、３次元環境復元方法、及びロボット
【課題】動的な環境下においても、効率的にマップ更新処理を可能とする。
【解決手段】３次元環境復元装置１０は、環境を３次元で計測する環境認識センサ５と、計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部１１と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、生成された３次元点群データを用いて多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部１２と、蓄積された規定時間分の多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部１３と、を備える。
【解決手段】３次元環境復元装置１０は、環境を３次元で計測する環境認識センサ５と、計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部１１と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、生成された３次元点群データを用いて多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部１２と、蓄積された規定時間分の多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部１３と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は３次元環境復元技術に関し、動的環境下でのマップ更新処理を効率的に実現可能な３次元環境復元装置、３次元環境復元方法、及びロボットに関する。
【背景技術】
【０００２】
３次元環境マップの表現形式として、例えば、非特許文献１又は２に開示される技術が知られている。非特許文献１に開示される３次元ボクセル形式では、３次元空間を等間隔の格子（グリッド）で分割し、各グリッドが持つ属性によってその近傍の空間の状態を表している。
【０００３】
非特許文献２に開示されるMulti Level Surface Map(以下、MLSMと略称する場合がある。)では、物体の最上部の表面をSurface pathとして保持すると共に、縦方向に複数の格子が並びこれらが同一物体に属する範囲であると考えられる場合には、これら複数の格子をまとめて一つの直方体構造により置き換えて表している。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】金広文男:"ヒューマノイドロボットの環境モデルと歩行計画3Dグリッドマップの利用",日本ロボット学会誌,Vol.26,No.4,pp.326-329,(2008).
【非特許文献２】R. Triebel, P. Pfaff, and W. Burgard "Multi-Level Surface Maps for Outdoor Terrain Mapping and Loop Closing" In Proc. of the International Conference on Intelligent Robots and Systems (IROS), 2006.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
非特許文献１に開示された３次元ボクセル形式では、対象とする環境（空間）の全てのグリッドについて物体の存在の有無を示す情報を持たせた上で、各グリッド単位のボクセルの集合により物体を表現するものであるため、メモリ消費量が膨大となる。
【０００６】
非特許文献２に開示されたMLSMによれば、３次元ボクセル形式と比較してメモリ消費量を少なくすることができる。しかしながら、非特許文献２に開示されたMLSMは、自己位置推定に用いる固定環境下での地図作成を目的とするものであり、また、障害物回避について何ら示唆及び記載されていないことからも、物体の位置や姿勢が動的に変化するような動的な環境下での地図更新については何ら考慮がされていないものである。このため、非特許文献２に開示されたMLSMを、動的な環境に対してそのまま適用した場合には、環境マップを更新する際には全マップ情報を一から更新する必要があり、リアルタイムでの処理が困難となることが予想される。従って、効率的にマップの更新をすることができないという問題がある。
【０００７】
従って本発明は、上述した課題を解決して、動的な環境下においても、効率的にマップ更新処理が可能な３次元環境復元装置、３次元環境復元方法、及びロボットを提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明の第１の態様にかかる３次元環境復元装置は、環境を３次元で計測する環境認識センサと、前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、を備えるものである。
【０００９】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１０】
また、前記全体多段マップ生成処理部は、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体を、前回生成した前記全体多段マップに追加して今回の全体多段マップを生成し、当該生成した全体多段マップから、前記記憶手段に蓄積された前記規定時間分の前記多段マップのうちで最も古い前記多段マップに含まれる直方体を削除するようにしてもよい。これにより、より効率的にマップ更新処理を実現することができる。
【００１１】
さらにまた、前記全体多段マップ生成処理部は、前記記憶手段に蓄積された前記規定時間分の前記多段マップに含まれる直方体と、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、前記規定時間分の前記多段マップに含まれる直方体の高さを修正するようにしてもよい。
【００１２】
また、前記全体多段マップ生成処理部は、前記全体多段マップを生成する際に、
最新の前記多段マップが追加されると共に、過去の前記多段マップのうちで最古の前記多段マップを削除するようにしてもよい。
【００１３】
さらにまた、前記環境認識センサは、観測物体までの距離データを取得するレーザレンジファインダであってもよい。
【００１４】
本発明の第２の態様にかかる３次元環境復元方法は、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、当該多段マップを生成及び蓄積する３次元環境復元方法であって、環境が３次元で計測された計測データを取得する環境認識ステップと、前記計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成ステップと、前記３次元点群データを用いて前記多段マップを時分割で生成する時分割多段マップ生成ステップと、前記多段マップを記憶手段に規定時間分蓄積する時分割多段マップ蓄積ステップと、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成ステップと、を有するものである。
【００１５】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１６】
本発明の第３の態様にかかるロボットは、環境を３次元で計測する環境認識センサを備え、当該環境認識センサで計測された計測データに基づいて生成する３次元環境地図を用いて動作を行うロボットであって、前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、を備えるものである。
【００１７】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１８】
また、前記時分割多段マップ生成／蓄積処理部は、前記３次元点群生成処理部で生成された３次元点群データを用いて第１の格子サイズの前記多段マップを時分割で生成し、当該生成した第１の格子サイズの多段マップを記憶手段に規定時間分蓄積し、前記３次元点群生成処理部で生成された３次元点群データを用いて前記第１の格子サイズよりも大きな第２の格子サイズの前記多段マップを時分割で生成し、当該生成した第２の格子サイズの多段マップを前記記憶手段に規定時間分蓄積し、前記全体多段マップ生成処理部は、前記記憶手段に蓄積された前記規定時間分の前記第１の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された記第１の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第１の格子サイズの全体多段マップ生成し、前記記憶手段に蓄積された前記規定時間分の前記第２の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された前記第２の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第２の格子サイズの全体多段マップ生成し、前記ロボットは、前記第１の格子サイズの全体多段マップ及び前記第２の格子サイズの全体多段マップを用途に応じて使い分けるようにしてもよい。
【発明の効果】
【００１９】
本発明によれば、動的な環境下においても、効率的にマップ更新処理が可能な３次元環境復元装置、３次元環境復元方法、及びロボットを提供することができる。
【図面の簡単な説明】
【００２０】
【図１】実施の形態１にかかるロボットの概略構成図である。
【図２】実施の形態１にかかる多段マップである。
【図３】実施の形態１にかかる３次元環境復元装置の構成図である。
【図４】実施の形態１にかかる環境計測の様子を説明するための図である。
【図５】実施の形態１にかかる３次元点群及び時分割多段マップの図である。
【図６】実施の形態１にかかる３次元点群及び時分割多段マップの図である。
【図７】実施の形態１にかかる全体多段マップの図である。
【図８】実施の形態１にかかる時分割多段マップの生成/蓄積処理のフローチャートである。
【図９】実施の形態１にかかる全体多段マップの生成処理のフローチャートである。
【図１０】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１１】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１２】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１３】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１４】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１５】実施の形態２にかかる全体多段マップの図である。
【発明を実施するための形態】
【００２１】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本実施の形態にかかるロボットの概略構成図である。
ロボット１は、胴体２と、首関節３を介して胴体２に連結された頭部４と、右アーム６と、左アーム７と、を備えている。さらに、ロボット１の頭部４には、環境認識センサとしてのレーザレンジファインダ５が備えられている。
【００２２】
レーザレンジファインダ５は、図に示すように、前方方向に向けてレーザを放射し、当該レーザを左右方向へと走査することで、観測物体までの距離データを取得する。そして、回転軸Ｃ１を中心として首関節３が回転することで、観測物体（例えばテーブル８）を含む前方の環境を走査することができる。
【００２３】
レーザレンジファインダ５が走査して得た計測データは、後述する３次元環境復元装置（不図示）へと転送される。３次元環境復元装置は、計測データに基づいて３次元環境を多段マップとして復元する。ロボット１は、復元された多段マップを利用して、環境と接触せずにアームを動作させる、ロボット１自身を移動させる等の制御を行う。
【００２４】
図２は、本実施の形態にかかる多段マップを例示する概念図である。
多段マップは、３次元環境を表現するデータ構造であり、メモリ消費量が少ないという利点を有している。本実施の形態では、３次元環境の表現形式として、Multi Level Surface Mapを簡素化した多段マップを用いている。
【００２５】
図２に示すように、多段マップでは、環境の基準平面２０を格子状に区切り、物体が存在する領域に対して直方体が割り当てられる。図では、直方体上面のｚ方向位置をＴｏｐとし、直方体の高さをＤｅｐｔｈとして示している。直方体の上面の位置を示す値及び高さを示す値は、任意の実数値を取ることができ、各直方体は、上面の位置Ｔｏｐ及び高さＤｅｐｔｈのデータを保持している。また、多段マップでは、環境内に複数の物体が存在する場合には、その配置状況に応じて高さ方向（ｚ方向）に複数の直方体を設定することができる。例えば図に示すように、直方体２１と直方体２２とを多段に設定することができる。
【００２６】
なお、非特許文献２に開示されたMLSMでは、物体の上面に対してその高さの平均値及び分散値を設定することができるが、本実施の形態では、これら値を設定せずに、より簡素化した多段マップを利用している。非特許文献２に開示されたMLSMでは自己位置推定を目的とするものであるため、高さの分散値を算出してこれを保持しておくことが非常に重要となる。本実施の形態では、高さの分散値の算出及び保持については省略することで、動的な環境下においてより効率的なマップ更新を可能としているが、これに限定されず、高さの分散値についても算出及び保持するようにしてもよい。
【００２７】
図３は、本実施の形態１にかかる３次元環境復元装置の構成図である。
３次元環境復元装置１０は、３次元点群生成処理部１１と、時分割多段マップ生成／蓄積処理部１２と、全体多段マップ生成処理部１３と、を備えている。３次元環境復元装置１０は、レーザレンジファインダ５から入力される計測データを記憶／処理することで、環境マップを生成する。生成された環境マップは、動的環境下での、ロボット１の障害物回避処理や、移動経路計画処理等に利用することができる。
【００２８】
３次元点群生成処理部１１は、レーザレンジファインダ５による計測データから、３次元点群データを生成する。ここで、３次元点群とは、ｘ、ｙ、ｚ方向の値を持つ３次元点の集合であり、観測物体の３次元形状を表している。レーザレンジファインダ５により計測された観測物体までの距離データから、各３次元点群のｘ、ｙ、ｚ値が算出される。
【００２９】
時分割多段マップ生成／蓄積処理部１２は、生成された３次元点群データを用いて、部分的な多段マップを生成する。本実施の形態では、レーザレンジファインダ５により環境が計測されるごとに生成する部分的な多段マップであって、時分割で生成する多段マップを、時分割多段マップと称する。時分割多段マップ生成／蓄積処理部１２は、新たに生成した時分割多段マップをメモリ等の記憶手段（不図示）に記憶する。
【００３０】
また、時分割多段マップ生成／蓄積処理部１２は、複数の時分割多段マップを記憶手段に蓄積する。本実施の形態では、過去の時分割多段マップを規定の時間分保持したものを、時分割多段マップ群と称する。また、このような保持された過去の時分割多段マップ群に対して、最新の時分割多段マップが追加される。
【００３１】
全体多段マップ生成処理部１３は、過去の時分割多段マップ群及び最新の時分割多段マップを合成することで、新たな環境情報を反映させた多段マップを生成する。最新の時分割多段マップが合成された多段マップを、全体多段マップと称する。
【００３２】
より具体的には、全体多段マップ生成処理部１３は、初回処理時には、過去の時分割多段マップ群を全て加算することで、全体多段マップを生成する。全体多段マップ生成処理部１３は、初回処理に続く以降の処理では、前回処理で生成した全体多段マップに最新の時分割多段マップを加算すると共に、当該全体多段マップから最も過去の時分割多段マップを減算する。本実施の形態では、時分割多段マップの追加及び削除はＦＩＦＯ（First In First Out）方式により行い、最新の時分割多段マップが追加されると共に、過去の時分割多段マップ群から最古の時分割多段マップが削除される。
【００３３】
なお、３次元環境復元装置１０は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。
【００３４】
次に、図４乃至７を参照して、３次元環境復元装置１０による多段マップの更新処理の概要について説明する。
まず、図４を参照して、レーザレンジファインダ５による環境計測の様子を説明する。同図に示すように、ロボット１の頭部４に設けられたレーザレンジファインダ５がチルト揺動されることで、テーブル８及びテーブル８上の物体９がスキャンされる。図に示す例では、テーブル８の手前方向から奥手方向へと向けてスキャンが行われ、１回目のスキャンをスキャン３１、これに続く２回目のスキャンをスキャン３２とする。
【００３５】
図５は、３次元点群及び時分割多段マップを例示する概念図である。スキャン３１（１回目のスキャン）による計測データに基づいて生成された３次元点群データの概念図を、図５（ａ）に示している。そして、同図（ａ）の３次元点群データに基づいて生成された時分割多段マップ４１の概念図を、図５（ｂ）に示している。
【００３６】
図６は、３次元点群及び時分割多段マップを例示する概念図である。スキャン３２（２回目のスキャン）による計測データに基づいて生成された３次元点群データの概念図を、図６（ａ）に示している。そして、同図（ａ）の３次元点群データに基づいて生成された時分割多段マップ４２の概念図を、図６（ｂ）に示している。
【００３７】
図７は、時分割多段マップ群から生成された全体多段マップを例示する概念図である。図５（ｂ）及び図６（ｂ）に示した時分割多段マップ４１及び４２に基づいて生成された全体多段マップ４３を、図７に示している。
【００３８】
なお、非特許文献２に開示されたMLSMでは、１回のチルト揺動により前方領域全てにわたるスキャンからの３次元点群データを取得し、これら取得した１回のチルト揺動分の３次元データ群を用いて、一度にMLSMを生成するものであると考えられる。これに対して本実施の形態では、１スキャンごとに１つの時分割多段マップを生成及び蓄積し、時分割多段マップを規定時間分蓄積させた後に、これらを合成して全体多段マップを生成する点で相違する。また、本実施の形態では、時分割多段マップを生成した時点で既に直方体構造により物体が表現されており、縦方向に直方体が重なっている場合にはこれを加算することで、時分割多段マップの合成を行うものである。
【００３９】
次に、図８乃至１４を参照して、３次元環境復元装置１０による多段マップの更新処理の詳細について説明する。
【００４０】
まず、図８を参照して、３次元環境復元装置１０による時分割多段マップの生成／蓄積処理について説明する。
【００４１】
Ｓ１０１において、環境マップ（全体多段マップ）を生成する領域が設定される。環境マップの生成領域は、レーザレンジファインダ５による環境計測が可能な範囲内で設定することができるが、例えば、ロボット１のアームの到達範囲などを設定して環境マップを生成するようにしてもよい。また、領域の設定は、ユーザにより、或いは３次元環境復元装置１０が自動的に設定することができる。
【００４２】
Ｓ１０２において、３次元点群生成処理部１１で生成された３次元点群が、時分割多段マップ生成／蓄積処理部１２に入力される。ここで、入力される３次元点群は、１回のスキャンに含まれる計測データのみから生成されている。なお、入力される３次元点群は、基準平面座標系に変換されている。基準平面座標系とは、基準平面が設定される基準座標系であり、基準座標系は任意の箇所に対して定めることができる。例えば、図４に示すようにテーブル８を含む環境を上から見下ろして走査する状況では、ロボット１の重心位置に基準座標系を定めるとよい。
【００４３】
Ｓ１０３において、時分割多段マップ生成／蓄積処理部１２は、入力された３次元点群の各３次元点についてのループ処理を開始する。このため、各３次元点について、以下のＳ１０４及びＳ１０５の処理が行われる。
【００４４】
Ｓ１０４において、時分割多段マップ生成／蓄積処理部１２は、３次元点のｘ、ｙ座標値から、当該３次元点が基準平面上のどの格子領域に含まれるかを特定する。
【００４５】
Ｓ１０５において、時分割多段マップ生成／蓄積処理部１２は、３次元点のｚ座標値における水平面を上面とする直方体を生成する。すなわち、Ｔｏｐを３次元点のｚ座標値とし、高さをＤｅｐｔｈとする直方体を生成する。なお、時分割多段マップ生成処理において直方体を生成する際には、予め定めた所定値をＤｅｐｔｈとする。ここでは、例えば、３次元点のｚ座標値における水平面を上面とする立方体を生成する。また、同一の格子領域内に同一のｚ座標値を持つ複数の３次元点が存在した場合には、１つの直方体を代表して生成すればよい。
【００４６】
Ｓ１０６において、時分割多段マップ生成／蓄積処理部１２は、Ｓ１０３〜Ｓ１０５までの処理によって新たに生成された直方体の集合（直方体群）を、現在の時間における時分割多段マップとして記憶手段に保持する。
【００４７】
次に、図９乃至１４を参照して、３次元環境復元装置１０による全体多段マップの生成処理について説明する。
図９は、全体多段マップの生成処理のフローチャートである。
【００４８】
Ｓ２０１において、時分割多段マップ生成／蓄積処理部１２で生成された時分割多段マップが、記憶手段に入力される。図１０（ａ）に示す例では、前々回生成された時分割多段マップと、前回生成された時分割多段マップと、が既に入力されており、今回生成された時分割多段マップが新たに入力されるようすを示している。図１０（ａ）に示す例では、前々回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ−２］と、前回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ−１］と、に対して、今回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ］が新たに追加されるようすを示している。図１０（ｂ）は、前回の全体多段マップ生成処理により生成された全体多段マップａｌｌ［ｔ−１］を示している。
【００４９】
なお、本実施の形態では、前回生成した全体多段マップに加えて、過去の時分割多段マップに含まれる直方体についても併せて記憶手段に保持しておく。これにより、今回の全体多段マップを更新するに際して、今回新たに追加する直方体を過去のどの直方体に対して修正、加算、削除すればよいのかを特定することができる。このため、全体多段マップ生成処理部１３は、前回の全体多段マップ生成処理の結果と前回の時分割多段マップを保持しており、図１０に示す例では、直方体ｐａｒｔ［ｔ−１］と、直方体ｐａｒｔ［ｔ−２］と、直方体ａｌｌ［ｔ−１］と、を保持している。
【００５０】
Ｓ２０２において、全体多段マップ生成処理部１３は、予め定めた規定時間分の時分割多段マップが記憶手段に蓄積されているか否かを判定し、蓄積されていない場合にはＳ２０１へと戻り、蓄積されている場合にはＳ２０３へと進む。
【００５１】
Ｓ２０３において、全体多段マップ生成処理部１３は、最新の時分割多段マップを用いて、過去の時分割多段マップを変形する。より具体的には、全体多段マップ生成処理部１３は、過去の時分割多段マップに含まれる直方体と、最新の時分割多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、過去の時分割多段マップに含まれる直方体の高さを修正する。図１１（ａ）に示す例では、前回の直方体ｐａｒｔ［ｔ−１］に対して、追加した今回の直方体ｐａｒｔ［ｔ］が重なっているため、同図（ｂ）に示すように、前回の直方体ｐａｒｔ［ｔ−１］の高さを修正する。
【００５２】
Ｓ２０４において、全体多段マップ生成処理部１３は、最新の時分割多段マップを、全体多段マップに加算する。より具体的には、全体多段マップ生成処理部１３は、最新の時分割多段マップに含まれる直方体を、前回の全体多段マップ生成処理により生成された全体多段マップに追加する。図１２に示す例では、同図（ａ）に示す今回の直方体ｐａｒｔ［ｔ］を、同図（ｂ）に示すように、前回の全体多段マップａｌｌ［ｔ−１］に追加し、追加した全体多段マップを今回の全体多段マップａｌｌ［ｔ］とする。
【００５３】
Ｓ２０５において、全体多段マップ生成処理部１３は、最も古い時分割多段マップを、全体多段マップから減算する。より具体的には、全体多段マップ生成処理部１３は、最も古い時分割多段マップに含まれる直方体を、今回の全体多段マップから削除する。なお、最古の直方体は全体多段マップから削除されると共に、最古の直方体そのものも記憶手段から削除される。図１３に示す例では、同図（ａ）に示す最古の直方体ｐａｒｔ［ｔ−２］を、同図（ｂ）に示すように、今回の全体多段マップａｌｌ［ｔ］から削除する。
【００５４】
なお、最古の時分割マップを減算することで、結果として、全体多段マップが分割されることもある。例えば、図１４に示すように、同図（ａ）に示す最古の直方体ｐａｒｔ［ｔ−２］を削除した結果、同図（ｂ）に示すように、前回の直方体ｐａｒｔ［ｔ−１］と、今回の直方体ｐａｒｔ［ｔ］とが分離することもある。
【００５５】
Ｓ２０６において、全体多段マップ生成処理部１３は、Ｓ２０３〜Ｓ２０５までの処理によって完成した今回の全体多段マップを出力する。このように、本実施の形態では、新たな時分割多段マップが入力される都度、それまでに保持していた全ての時分割多段マップを用いて新たな全体多段マップを作り直すのではなく、最新の時分割多段マップと最古の時分割多段マップとの入れ替えによって、全体多段マップを更新していく。このようなＦＩＦＯ構造によって、マップ更新処理の高速化を実現することができる。
【００５６】
また、全体多段マップ生成処理部１３は、規定時間分の時分割多段マップと、全体多段マップの両方を記憶手段に保持しているが、例えばアームの軌道計画を行う場合には、時分割多段マップ群ではなく今回生成した全体多段マップを用いることで、より効率的に動的な環境下での軌道計画が可能となる。これは、図１３に示した例示したように、時分割多段マップ群に含まれる全ての直方体を利用するよりも、全体多段マップに含まれる複数の直方体をまとめた後の直方体を利用したほうが、計算対象となる直方体のデータ量を削減することができ、より効率的に軌道計画が可能となるためである。
【００５７】
以上説明したように、本実施の形態によれば、それぞれの時点で生成した時分割多段マップを規定時間分蓄積し、時分割多段マップ群を用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理が可能となる。
また、メモリ消費量の少ない３次元環境マップを生成することができるため、限られた計算機資源を有効活用することができる。
さらに、メモリ消費量の少ない、つまり、データ量の少ない３次元環境マップが生成されるため、本マップを用いてアームの軌道計画や移動経路計画する際には、計算時間を短く抑えることができる。
【００５８】
実施の形態２．
次に、本発明の実施の形態２について説明する。
上述の実施の形態においては、１つの全体多段マップを生成するものであったが、本実施の形態においては、複数の全体多段マップを同時に生成するものである（ここでは２つの全体多段マップを例示する）。なお、図１に示したロボット１の概略構成や、図２に示した３次元環境復元装置１０の構成及び処理については上述した実施の形態１と同一であるためその詳細な説明を省略し、以下では、実施の形態１と異なる点を中心に説明する。
【００５９】
３次元環境復元装置１０により生成される３次元環境マップ（全体多段マップ）は、低容量のデータ構造である。このため、格子サイズの異なる複数の３次元環境マップを同時に生成及び保持することができ、これら複数のマップをそれぞれ用途に応じて生成及び保持することで、アームの軌道計画やロボット１の移動経路計画に利用することが可能となる。
【００６０】
本実施の形態にかかる３次元環境復元装置１０は、各３次元点群から第１の格子サイズの時分割多段マップを生成してこれらを蓄積し、蓄積された時分割多段マップ群を用いて第１の全体多段マップを生成する。同時に、第１の全体多段マップの生成に用いたものと同じ各３次元点群から、第１の格子サイズよりも大きな第２の格子サイズの時分割多段マップを生成してこれらを蓄積し、蓄積された時分割多段マップ群を用いて第２の全体多段マップを生成する。
【００６１】
例えば、図１５（ａ）に示すような格子サイズのより大きな全体多段マップを、自律移動の際の平面検出に用いるため生成し、同図（ｂ）に示すような格子サイズのより小さな全体多段マップを、物体把持のために物体形状の把握に用いるために生成することができる。図１５（ａ）に示すような格子サイズのより大きな全体多段マップを用いて平面検出を行うことで、３次元点群に抜けがある場合においても、同一の格子領域内に含まれる他の同一の高さの３次元点によって平面が特定されやすくなり、平面検出をより精度良く行うことができる。また、図１５（ｂ）に示すような格子サイズのより小さな全体多段マップを用いることで、把持対象となる物体の形状をより正確に把握することができる。
【００６２】
また、例えば、ロボット１に扉の開閉動作を実行させる場合には、ロボット１は現在位置から扉付近に近づくように移動すると共に、アームを延ばしながらハンド等によって扉の取っ手を把持することが求められる。このため、移動経路計画用のより大きな格子サイズの全体多段マップと、アームの軌道計画用のより詳細な全体多段マップとの両方を同時に生成及び保持することで、より効率的に動作を実行させることが可能となる。
【００６３】
その他の実施の形態．
本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上述の実施の形態では、２本のアームを備えたロボット１の上体部を例示したが、ロボット１は、車輪や脚などの移動手段を備えていても良い。また、ロボット１に限定されず、車などの移動体に本発明を適用するものとしてもよい。
【００６４】
また、例えば、上述の実施の形態では、環境認識センサとしてレーザレンジファインダを用いた場合を例に説明したが本発明はこれに限定されず、環境認識センサは、環境を３次元で計測可能な公知の手段を採用することができ、例えばカメラ等を用いて３次元点群を生成するためのデータを取得するものとしてもよい。
【００６５】
また、例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、にコンピュータプログラムを実行させることにより実現することも可能である。
【符号の説明】
【００６６】
１ ロボット、
２ 胴体、
３ 首関節、
４ 頭部、
５ レーザレンジファインダ、
６ 右アーム、
７ 左アーム、
８ テーブル、
９ 物体、
１０ ３次元環境復元装置、
１１ ３次元点群生成処理部、
１２ 時分割多段マップ生成／蓄積処理部、
１３ 全体多段マップ生成処理部、
２０ 基準平面、
２１、 ２２ 直方体、
４１、４２ 時分割多段マップ、
４３ 全体多段マップ、
【技術分野】
【０００１】
本発明は３次元環境復元技術に関し、動的環境下でのマップ更新処理を効率的に実現可能な３次元環境復元装置、３次元環境復元方法、及びロボットに関する。
【背景技術】
【０００２】
３次元環境マップの表現形式として、例えば、非特許文献１又は２に開示される技術が知られている。非特許文献１に開示される３次元ボクセル形式では、３次元空間を等間隔の格子（グリッド）で分割し、各グリッドが持つ属性によってその近傍の空間の状態を表している。
【０００３】
非特許文献２に開示されるMulti Level Surface Map(以下、MLSMと略称する場合がある。)では、物体の最上部の表面をSurface pathとして保持すると共に、縦方向に複数の格子が並びこれらが同一物体に属する範囲であると考えられる場合には、これら複数の格子をまとめて一つの直方体構造により置き換えて表している。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】金広文男:"ヒューマノイドロボットの環境モデルと歩行計画3Dグリッドマップの利用",日本ロボット学会誌,Vol.26,No.4,pp.326-329,(2008).
【非特許文献２】R. Triebel, P. Pfaff, and W. Burgard "Multi-Level Surface Maps for Outdoor Terrain Mapping and Loop Closing" In Proc. of the International Conference on Intelligent Robots and Systems (IROS), 2006.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
非特許文献１に開示された３次元ボクセル形式では、対象とする環境（空間）の全てのグリッドについて物体の存在の有無を示す情報を持たせた上で、各グリッド単位のボクセルの集合により物体を表現するものであるため、メモリ消費量が膨大となる。
【０００６】
非特許文献２に開示されたMLSMによれば、３次元ボクセル形式と比較してメモリ消費量を少なくすることができる。しかしながら、非特許文献２に開示されたMLSMは、自己位置推定に用いる固定環境下での地図作成を目的とするものであり、また、障害物回避について何ら示唆及び記載されていないことからも、物体の位置や姿勢が動的に変化するような動的な環境下での地図更新については何ら考慮がされていないものである。このため、非特許文献２に開示されたMLSMを、動的な環境に対してそのまま適用した場合には、環境マップを更新する際には全マップ情報を一から更新する必要があり、リアルタイムでの処理が困難となることが予想される。従って、効率的にマップの更新をすることができないという問題がある。
【０００７】
従って本発明は、上述した課題を解決して、動的な環境下においても、効率的にマップ更新処理が可能な３次元環境復元装置、３次元環境復元方法、及びロボットを提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明の第１の態様にかかる３次元環境復元装置は、環境を３次元で計測する環境認識センサと、前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、を備えるものである。
【０００９】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１０】
また、前記全体多段マップ生成処理部は、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体を、前回生成した前記全体多段マップに追加して今回の全体多段マップを生成し、当該生成した全体多段マップから、前記記憶手段に蓄積された前記規定時間分の前記多段マップのうちで最も古い前記多段マップに含まれる直方体を削除するようにしてもよい。これにより、より効率的にマップ更新処理を実現することができる。
【００１１】
さらにまた、前記全体多段マップ生成処理部は、前記記憶手段に蓄積された前記規定時間分の前記多段マップに含まれる直方体と、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、前記規定時間分の前記多段マップに含まれる直方体の高さを修正するようにしてもよい。
【００１２】
また、前記全体多段マップ生成処理部は、前記全体多段マップを生成する際に、
最新の前記多段マップが追加されると共に、過去の前記多段マップのうちで最古の前記多段マップを削除するようにしてもよい。
【００１３】
さらにまた、前記環境認識センサは、観測物体までの距離データを取得するレーザレンジファインダであってもよい。
【００１４】
本発明の第２の態様にかかる３次元環境復元方法は、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、当該多段マップを生成及び蓄積する３次元環境復元方法であって、環境が３次元で計測された計測データを取得する環境認識ステップと、前記計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成ステップと、前記３次元点群データを用いて前記多段マップを時分割で生成する時分割多段マップ生成ステップと、前記多段マップを記憶手段に規定時間分蓄積する時分割多段マップ蓄積ステップと、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成ステップと、を有するものである。
【００１５】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１６】
本発明の第３の態様にかかるロボットは、環境を３次元で計測する環境認識センサを備え、当該環境認識センサで計測された計測データに基づいて生成する３次元環境地図を用いて動作を行うロボットであって、前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、を備えるものである。
【００１７】
これにより、このように、時分割で生成した多段マップを規定時間分蓄積し、蓄積した多段マップと新たに生成された多段マップを用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理を実現することができる。
【００１８】
また、前記時分割多段マップ生成／蓄積処理部は、前記３次元点群生成処理部で生成された３次元点群データを用いて第１の格子サイズの前記多段マップを時分割で生成し、当該生成した第１の格子サイズの多段マップを記憶手段に規定時間分蓄積し、前記３次元点群生成処理部で生成された３次元点群データを用いて前記第１の格子サイズよりも大きな第２の格子サイズの前記多段マップを時分割で生成し、当該生成した第２の格子サイズの多段マップを前記記憶手段に規定時間分蓄積し、前記全体多段マップ生成処理部は、前記記憶手段に蓄積された前記規定時間分の前記第１の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された記第１の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第１の格子サイズの全体多段マップ生成し、前記記憶手段に蓄積された前記規定時間分の前記第２の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された前記第２の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第２の格子サイズの全体多段マップ生成し、前記ロボットは、前記第１の格子サイズの全体多段マップ及び前記第２の格子サイズの全体多段マップを用途に応じて使い分けるようにしてもよい。
【発明の効果】
【００１９】
本発明によれば、動的な環境下においても、効率的にマップ更新処理が可能な３次元環境復元装置、３次元環境復元方法、及びロボットを提供することができる。
【図面の簡単な説明】
【００２０】
【図１】実施の形態１にかかるロボットの概略構成図である。
【図２】実施の形態１にかかる多段マップである。
【図３】実施の形態１にかかる３次元環境復元装置の構成図である。
【図４】実施の形態１にかかる環境計測の様子を説明するための図である。
【図５】実施の形態１にかかる３次元点群及び時分割多段マップの図である。
【図６】実施の形態１にかかる３次元点群及び時分割多段マップの図である。
【図７】実施の形態１にかかる全体多段マップの図である。
【図８】実施の形態１にかかる時分割多段マップの生成/蓄積処理のフローチャートである。
【図９】実施の形態１にかかる全体多段マップの生成処理のフローチャートである。
【図１０】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１１】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１２】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１３】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１４】実施の形態１にかかる全体多段マップの生成処理の様子を説明するための図である。
【図１５】実施の形態２にかかる全体多段マップの図である。
【発明を実施するための形態】
【００２１】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本実施の形態にかかるロボットの概略構成図である。
ロボット１は、胴体２と、首関節３を介して胴体２に連結された頭部４と、右アーム６と、左アーム７と、を備えている。さらに、ロボット１の頭部４には、環境認識センサとしてのレーザレンジファインダ５が備えられている。
【００２２】
レーザレンジファインダ５は、図に示すように、前方方向に向けてレーザを放射し、当該レーザを左右方向へと走査することで、観測物体までの距離データを取得する。そして、回転軸Ｃ１を中心として首関節３が回転することで、観測物体（例えばテーブル８）を含む前方の環境を走査することができる。
【００２３】
レーザレンジファインダ５が走査して得た計測データは、後述する３次元環境復元装置（不図示）へと転送される。３次元環境復元装置は、計測データに基づいて３次元環境を多段マップとして復元する。ロボット１は、復元された多段マップを利用して、環境と接触せずにアームを動作させる、ロボット１自身を移動させる等の制御を行う。
【００２４】
図２は、本実施の形態にかかる多段マップを例示する概念図である。
多段マップは、３次元環境を表現するデータ構造であり、メモリ消費量が少ないという利点を有している。本実施の形態では、３次元環境の表現形式として、Multi Level Surface Mapを簡素化した多段マップを用いている。
【００２５】
図２に示すように、多段マップでは、環境の基準平面２０を格子状に区切り、物体が存在する領域に対して直方体が割り当てられる。図では、直方体上面のｚ方向位置をＴｏｐとし、直方体の高さをＤｅｐｔｈとして示している。直方体の上面の位置を示す値及び高さを示す値は、任意の実数値を取ることができ、各直方体は、上面の位置Ｔｏｐ及び高さＤｅｐｔｈのデータを保持している。また、多段マップでは、環境内に複数の物体が存在する場合には、その配置状況に応じて高さ方向（ｚ方向）に複数の直方体を設定することができる。例えば図に示すように、直方体２１と直方体２２とを多段に設定することができる。
【００２６】
なお、非特許文献２に開示されたMLSMでは、物体の上面に対してその高さの平均値及び分散値を設定することができるが、本実施の形態では、これら値を設定せずに、より簡素化した多段マップを利用している。非特許文献２に開示されたMLSMでは自己位置推定を目的とするものであるため、高さの分散値を算出してこれを保持しておくことが非常に重要となる。本実施の形態では、高さの分散値の算出及び保持については省略することで、動的な環境下においてより効率的なマップ更新を可能としているが、これに限定されず、高さの分散値についても算出及び保持するようにしてもよい。
【００２７】
図３は、本実施の形態１にかかる３次元環境復元装置の構成図である。
３次元環境復元装置１０は、３次元点群生成処理部１１と、時分割多段マップ生成／蓄積処理部１２と、全体多段マップ生成処理部１３と、を備えている。３次元環境復元装置１０は、レーザレンジファインダ５から入力される計測データを記憶／処理することで、環境マップを生成する。生成された環境マップは、動的環境下での、ロボット１の障害物回避処理や、移動経路計画処理等に利用することができる。
【００２８】
３次元点群生成処理部１１は、レーザレンジファインダ５による計測データから、３次元点群データを生成する。ここで、３次元点群とは、ｘ、ｙ、ｚ方向の値を持つ３次元点の集合であり、観測物体の３次元形状を表している。レーザレンジファインダ５により計測された観測物体までの距離データから、各３次元点群のｘ、ｙ、ｚ値が算出される。
【００２９】
時分割多段マップ生成／蓄積処理部１２は、生成された３次元点群データを用いて、部分的な多段マップを生成する。本実施の形態では、レーザレンジファインダ５により環境が計測されるごとに生成する部分的な多段マップであって、時分割で生成する多段マップを、時分割多段マップと称する。時分割多段マップ生成／蓄積処理部１２は、新たに生成した時分割多段マップをメモリ等の記憶手段（不図示）に記憶する。
【００３０】
また、時分割多段マップ生成／蓄積処理部１２は、複数の時分割多段マップを記憶手段に蓄積する。本実施の形態では、過去の時分割多段マップを規定の時間分保持したものを、時分割多段マップ群と称する。また、このような保持された過去の時分割多段マップ群に対して、最新の時分割多段マップが追加される。
【００３１】
全体多段マップ生成処理部１３は、過去の時分割多段マップ群及び最新の時分割多段マップを合成することで、新たな環境情報を反映させた多段マップを生成する。最新の時分割多段マップが合成された多段マップを、全体多段マップと称する。
【００３２】
より具体的には、全体多段マップ生成処理部１３は、初回処理時には、過去の時分割多段マップ群を全て加算することで、全体多段マップを生成する。全体多段マップ生成処理部１３は、初回処理に続く以降の処理では、前回処理で生成した全体多段マップに最新の時分割多段マップを加算すると共に、当該全体多段マップから最も過去の時分割多段マップを減算する。本実施の形態では、時分割多段マップの追加及び削除はＦＩＦＯ（First In First Out）方式により行い、最新の時分割多段マップが追加されると共に、過去の時分割多段マップ群から最古の時分割多段マップが削除される。
【００３３】
なお、３次元環境復元装置１０は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。
【００３４】
次に、図４乃至７を参照して、３次元環境復元装置１０による多段マップの更新処理の概要について説明する。
まず、図４を参照して、レーザレンジファインダ５による環境計測の様子を説明する。同図に示すように、ロボット１の頭部４に設けられたレーザレンジファインダ５がチルト揺動されることで、テーブル８及びテーブル８上の物体９がスキャンされる。図に示す例では、テーブル８の手前方向から奥手方向へと向けてスキャンが行われ、１回目のスキャンをスキャン３１、これに続く２回目のスキャンをスキャン３２とする。
【００３５】
図５は、３次元点群及び時分割多段マップを例示する概念図である。スキャン３１（１回目のスキャン）による計測データに基づいて生成された３次元点群データの概念図を、図５（ａ）に示している。そして、同図（ａ）の３次元点群データに基づいて生成された時分割多段マップ４１の概念図を、図５（ｂ）に示している。
【００３６】
図６は、３次元点群及び時分割多段マップを例示する概念図である。スキャン３２（２回目のスキャン）による計測データに基づいて生成された３次元点群データの概念図を、図６（ａ）に示している。そして、同図（ａ）の３次元点群データに基づいて生成された時分割多段マップ４２の概念図を、図６（ｂ）に示している。
【００３７】
図７は、時分割多段マップ群から生成された全体多段マップを例示する概念図である。図５（ｂ）及び図６（ｂ）に示した時分割多段マップ４１及び４２に基づいて生成された全体多段マップ４３を、図７に示している。
【００３８】
なお、非特許文献２に開示されたMLSMでは、１回のチルト揺動により前方領域全てにわたるスキャンからの３次元点群データを取得し、これら取得した１回のチルト揺動分の３次元データ群を用いて、一度にMLSMを生成するものであると考えられる。これに対して本実施の形態では、１スキャンごとに１つの時分割多段マップを生成及び蓄積し、時分割多段マップを規定時間分蓄積させた後に、これらを合成して全体多段マップを生成する点で相違する。また、本実施の形態では、時分割多段マップを生成した時点で既に直方体構造により物体が表現されており、縦方向に直方体が重なっている場合にはこれを加算することで、時分割多段マップの合成を行うものである。
【００３９】
次に、図８乃至１４を参照して、３次元環境復元装置１０による多段マップの更新処理の詳細について説明する。
【００４０】
まず、図８を参照して、３次元環境復元装置１０による時分割多段マップの生成／蓄積処理について説明する。
【００４１】
Ｓ１０１において、環境マップ（全体多段マップ）を生成する領域が設定される。環境マップの生成領域は、レーザレンジファインダ５による環境計測が可能な範囲内で設定することができるが、例えば、ロボット１のアームの到達範囲などを設定して環境マップを生成するようにしてもよい。また、領域の設定は、ユーザにより、或いは３次元環境復元装置１０が自動的に設定することができる。
【００４２】
Ｓ１０２において、３次元点群生成処理部１１で生成された３次元点群が、時分割多段マップ生成／蓄積処理部１２に入力される。ここで、入力される３次元点群は、１回のスキャンに含まれる計測データのみから生成されている。なお、入力される３次元点群は、基準平面座標系に変換されている。基準平面座標系とは、基準平面が設定される基準座標系であり、基準座標系は任意の箇所に対して定めることができる。例えば、図４に示すようにテーブル８を含む環境を上から見下ろして走査する状況では、ロボット１の重心位置に基準座標系を定めるとよい。
【００４３】
Ｓ１０３において、時分割多段マップ生成／蓄積処理部１２は、入力された３次元点群の各３次元点についてのループ処理を開始する。このため、各３次元点について、以下のＳ１０４及びＳ１０５の処理が行われる。
【００４４】
Ｓ１０４において、時分割多段マップ生成／蓄積処理部１２は、３次元点のｘ、ｙ座標値から、当該３次元点が基準平面上のどの格子領域に含まれるかを特定する。
【００４５】
Ｓ１０５において、時分割多段マップ生成／蓄積処理部１２は、３次元点のｚ座標値における水平面を上面とする直方体を生成する。すなわち、Ｔｏｐを３次元点のｚ座標値とし、高さをＤｅｐｔｈとする直方体を生成する。なお、時分割多段マップ生成処理において直方体を生成する際には、予め定めた所定値をＤｅｐｔｈとする。ここでは、例えば、３次元点のｚ座標値における水平面を上面とする立方体を生成する。また、同一の格子領域内に同一のｚ座標値を持つ複数の３次元点が存在した場合には、１つの直方体を代表して生成すればよい。
【００４６】
Ｓ１０６において、時分割多段マップ生成／蓄積処理部１２は、Ｓ１０３〜Ｓ１０５までの処理によって新たに生成された直方体の集合（直方体群）を、現在の時間における時分割多段マップとして記憶手段に保持する。
【００４７】
次に、図９乃至１４を参照して、３次元環境復元装置１０による全体多段マップの生成処理について説明する。
図９は、全体多段マップの生成処理のフローチャートである。
【００４８】
Ｓ２０１において、時分割多段マップ生成／蓄積処理部１２で生成された時分割多段マップが、記憶手段に入力される。図１０（ａ）に示す例では、前々回生成された時分割多段マップと、前回生成された時分割多段マップと、が既に入力されており、今回生成された時分割多段マップが新たに入力されるようすを示している。図１０（ａ）に示す例では、前々回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ−２］と、前回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ−１］と、に対して、今回生成された時分割多段マップに含まれる直方体ｐａｒｔ［ｔ］が新たに追加されるようすを示している。図１０（ｂ）は、前回の全体多段マップ生成処理により生成された全体多段マップａｌｌ［ｔ−１］を示している。
【００４９】
なお、本実施の形態では、前回生成した全体多段マップに加えて、過去の時分割多段マップに含まれる直方体についても併せて記憶手段に保持しておく。これにより、今回の全体多段マップを更新するに際して、今回新たに追加する直方体を過去のどの直方体に対して修正、加算、削除すればよいのかを特定することができる。このため、全体多段マップ生成処理部１３は、前回の全体多段マップ生成処理の結果と前回の時分割多段マップを保持しており、図１０に示す例では、直方体ｐａｒｔ［ｔ−１］と、直方体ｐａｒｔ［ｔ−２］と、直方体ａｌｌ［ｔ−１］と、を保持している。
【００５０】
Ｓ２０２において、全体多段マップ生成処理部１３は、予め定めた規定時間分の時分割多段マップが記憶手段に蓄積されているか否かを判定し、蓄積されていない場合にはＳ２０１へと戻り、蓄積されている場合にはＳ２０３へと進む。
【００５１】
Ｓ２０３において、全体多段マップ生成処理部１３は、最新の時分割多段マップを用いて、過去の時分割多段マップを変形する。より具体的には、全体多段マップ生成処理部１３は、過去の時分割多段マップに含まれる直方体と、最新の時分割多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、過去の時分割多段マップに含まれる直方体の高さを修正する。図１１（ａ）に示す例では、前回の直方体ｐａｒｔ［ｔ−１］に対して、追加した今回の直方体ｐａｒｔ［ｔ］が重なっているため、同図（ｂ）に示すように、前回の直方体ｐａｒｔ［ｔ−１］の高さを修正する。
【００５２】
Ｓ２０４において、全体多段マップ生成処理部１３は、最新の時分割多段マップを、全体多段マップに加算する。より具体的には、全体多段マップ生成処理部１３は、最新の時分割多段マップに含まれる直方体を、前回の全体多段マップ生成処理により生成された全体多段マップに追加する。図１２に示す例では、同図（ａ）に示す今回の直方体ｐａｒｔ［ｔ］を、同図（ｂ）に示すように、前回の全体多段マップａｌｌ［ｔ−１］に追加し、追加した全体多段マップを今回の全体多段マップａｌｌ［ｔ］とする。
【００５３】
Ｓ２０５において、全体多段マップ生成処理部１３は、最も古い時分割多段マップを、全体多段マップから減算する。より具体的には、全体多段マップ生成処理部１３は、最も古い時分割多段マップに含まれる直方体を、今回の全体多段マップから削除する。なお、最古の直方体は全体多段マップから削除されると共に、最古の直方体そのものも記憶手段から削除される。図１３に示す例では、同図（ａ）に示す最古の直方体ｐａｒｔ［ｔ−２］を、同図（ｂ）に示すように、今回の全体多段マップａｌｌ［ｔ］から削除する。
【００５４】
なお、最古の時分割マップを減算することで、結果として、全体多段マップが分割されることもある。例えば、図１４に示すように、同図（ａ）に示す最古の直方体ｐａｒｔ［ｔ−２］を削除した結果、同図（ｂ）に示すように、前回の直方体ｐａｒｔ［ｔ−１］と、今回の直方体ｐａｒｔ［ｔ］とが分離することもある。
【００５５】
Ｓ２０６において、全体多段マップ生成処理部１３は、Ｓ２０３〜Ｓ２０５までの処理によって完成した今回の全体多段マップを出力する。このように、本実施の形態では、新たな時分割多段マップが入力される都度、それまでに保持していた全ての時分割多段マップを用いて新たな全体多段マップを作り直すのではなく、最新の時分割多段マップと最古の時分割多段マップとの入れ替えによって、全体多段マップを更新していく。このようなＦＩＦＯ構造によって、マップ更新処理の高速化を実現することができる。
【００５６】
また、全体多段マップ生成処理部１３は、規定時間分の時分割多段マップと、全体多段マップの両方を記憶手段に保持しているが、例えばアームの軌道計画を行う場合には、時分割多段マップ群ではなく今回生成した全体多段マップを用いることで、より効率的に動的な環境下での軌道計画が可能となる。これは、図１３に示した例示したように、時分割多段マップ群に含まれる全ての直方体を利用するよりも、全体多段マップに含まれる複数の直方体をまとめた後の直方体を利用したほうが、計算対象となる直方体のデータ量を削減することができ、より効率的に軌道計画が可能となるためである。
【００５７】
以上説明したように、本実施の形態によれば、それぞれの時点で生成した時分割多段マップを規定時間分蓄積し、時分割多段マップ群を用いて全体多段マップを更新していくことで、動的な環境下においても、効率的にマップ更新処理が可能となる。
また、メモリ消費量の少ない３次元環境マップを生成することができるため、限られた計算機資源を有効活用することができる。
さらに、メモリ消費量の少ない、つまり、データ量の少ない３次元環境マップが生成されるため、本マップを用いてアームの軌道計画や移動経路計画する際には、計算時間を短く抑えることができる。
【００５８】
実施の形態２．
次に、本発明の実施の形態２について説明する。
上述の実施の形態においては、１つの全体多段マップを生成するものであったが、本実施の形態においては、複数の全体多段マップを同時に生成するものである（ここでは２つの全体多段マップを例示する）。なお、図１に示したロボット１の概略構成や、図２に示した３次元環境復元装置１０の構成及び処理については上述した実施の形態１と同一であるためその詳細な説明を省略し、以下では、実施の形態１と異なる点を中心に説明する。
【００５９】
３次元環境復元装置１０により生成される３次元環境マップ（全体多段マップ）は、低容量のデータ構造である。このため、格子サイズの異なる複数の３次元環境マップを同時に生成及び保持することができ、これら複数のマップをそれぞれ用途に応じて生成及び保持することで、アームの軌道計画やロボット１の移動経路計画に利用することが可能となる。
【００６０】
本実施の形態にかかる３次元環境復元装置１０は、各３次元点群から第１の格子サイズの時分割多段マップを生成してこれらを蓄積し、蓄積された時分割多段マップ群を用いて第１の全体多段マップを生成する。同時に、第１の全体多段マップの生成に用いたものと同じ各３次元点群から、第１の格子サイズよりも大きな第２の格子サイズの時分割多段マップを生成してこれらを蓄積し、蓄積された時分割多段マップ群を用いて第２の全体多段マップを生成する。
【００６１】
例えば、図１５（ａ）に示すような格子サイズのより大きな全体多段マップを、自律移動の際の平面検出に用いるため生成し、同図（ｂ）に示すような格子サイズのより小さな全体多段マップを、物体把持のために物体形状の把握に用いるために生成することができる。図１５（ａ）に示すような格子サイズのより大きな全体多段マップを用いて平面検出を行うことで、３次元点群に抜けがある場合においても、同一の格子領域内に含まれる他の同一の高さの３次元点によって平面が特定されやすくなり、平面検出をより精度良く行うことができる。また、図１５（ｂ）に示すような格子サイズのより小さな全体多段マップを用いることで、把持対象となる物体の形状をより正確に把握することができる。
【００６２】
また、例えば、ロボット１に扉の開閉動作を実行させる場合には、ロボット１は現在位置から扉付近に近づくように移動すると共に、アームを延ばしながらハンド等によって扉の取っ手を把持することが求められる。このため、移動経路計画用のより大きな格子サイズの全体多段マップと、アームの軌道計画用のより詳細な全体多段マップとの両方を同時に生成及び保持することで、より効率的に動作を実行させることが可能となる。
【００６３】
その他の実施の形態．
本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上述の実施の形態では、２本のアームを備えたロボット１の上体部を例示したが、ロボット１は、車輪や脚などの移動手段を備えていても良い。また、ロボット１に限定されず、車などの移動体に本発明を適用するものとしてもよい。
【００６４】
また、例えば、上述の実施の形態では、環境認識センサとしてレーザレンジファインダを用いた場合を例に説明したが本発明はこれに限定されず、環境認識センサは、環境を３次元で計測可能な公知の手段を採用することができ、例えばカメラ等を用いて３次元点群を生成するためのデータを取得するものとしてもよい。
【００６５】
また、例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、にコンピュータプログラムを実行させることにより実現することも可能である。
【符号の説明】
【００６６】
１ ロボット、
２ 胴体、
３ 首関節、
４ 頭部、
５ レーザレンジファインダ、
６ 右アーム、
７ 左アーム、
８ テーブル、
９ 物体、
１０ ３次元環境復元装置、
１１ ３次元点群生成処理部、
１２ 時分割多段マップ生成／蓄積処理部、
１３ 全体多段マップ生成処理部、
２０ 基準平面、
２１、 ２２ 直方体、
４１、４２ 時分割多段マップ、
４３ 全体多段マップ、
【特許請求の範囲】
【請求項１】
環境を３次元で計測する環境認識センサと、
前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、
を備える３次元環境復元装置。
【請求項２】
前記全体多段マップ生成処理部は、
前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体を、前回生成した前記全体多段マップに追加して今回の全体多段マップを生成し、
当該生成した全体多段マップから、前記記憶手段に蓄積された前記規定時間分の前記多段マップのうちで最も古い前記多段マップに含まれる直方体を削除する
ことを特徴とする請求項１に記載の３次元環境復元装置。
【請求項３】
前記全体多段マップ生成処理部は、
前記記憶手段に蓄積された前記規定時間分の前記多段マップに含まれる直方体と、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、前記規定時間分の前記多段マップに含まれる直方体の高さを修正する
ことを特徴とする請求項２に記載の３次元環境復元装置。
【請求項４】
前記全体多段マップ生成処理部は、
前記全体多段マップを生成する際に、
最新の前記多段マップが追加されると共に、過去の前記多段マップのうちで最古の前記多段マップを削除する
ことを特徴とする請求項１乃至３いずれか１項に記載の３次元環境復元装置。
【請求項５】
前記環境認識センサは、
観測物体までの距離データを取得するレーザレンジファインダである
ことを特徴とする請求項１乃至４いずれか１項に記載の３次元環境復元装置。
【請求項６】
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、当該多段マップを生成及び蓄積する３次元環境復元方法であって、
環境が３次元で計測された計測データを取得する環境認識ステップと、
前記計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成ステップと、
前記３次元点群データを用いて前記多段マップを時分割で生成する時分割多段マップ生成ステップと、
前記多段マップを記憶手段に規定時間分蓄積する時分割多段マップ蓄積ステップと、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成ステップと、
を有する３次元環境復元方法。
【請求項７】
環境を３次元で計測する環境認識センサを備え、当該環境認識センサで計測された計測データに基づいて生成する３次元環境地図を用いて動作を行うロボットであって、
前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、
を備えるロボット。
【請求項８】
前記時分割多段マップ生成／蓄積処理部は、
前記３次元点群生成処理部で生成された３次元点群データを用いて第１の格子サイズの前記多段マップを時分割で生成し、当該生成した第１の格子サイズの多段マップを記憶手段に規定時間分蓄積し、
前記３次元点群生成処理部で生成された３次元点群データを用いて前記第１の格子サイズよりも大きな第２の格子サイズの前記多段マップを時分割で生成し、当該生成した第２の格子サイズの多段マップを前記記憶手段に規定時間分蓄積し、
前記全体多段マップ生成処理部は、
前記記憶手段に蓄積された前記規定時間分の前記第１の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された記第１の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第１の格子サイズの全体多段マップ生成し、
前記記憶手段に蓄積された前記規定時間分の前記第２の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された前記第２の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第２の格子サイズの全体多段マップ生成し、
前記ロボットは、前記第１の格子サイズの全体多段マップ及び前記第２の格子サイズの全体多段マップを用途に応じて使い分ける
ことを特徴とする請求項７記載のロボット。
【請求項１】
環境を３次元で計測する環境認識センサと、
前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、
を備える３次元環境復元装置。
【請求項２】
前記全体多段マップ生成処理部は、
前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体を、前回生成した前記全体多段マップに追加して今回の全体多段マップを生成し、
当該生成した全体多段マップから、前記記憶手段に蓄積された前記規定時間分の前記多段マップのうちで最も古い前記多段マップに含まれる直方体を削除する
ことを特徴とする請求項１に記載の３次元環境復元装置。
【請求項３】
前記全体多段マップ生成処理部は、
前記記憶手段に蓄積された前記規定時間分の前記多段マップに含まれる直方体と、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップに含まれる直方体との重なりを判定し、重なっていると判定した場合には、前記規定時間分の前記多段マップに含まれる直方体の高さを修正する
ことを特徴とする請求項２に記載の３次元環境復元装置。
【請求項４】
前記全体多段マップ生成処理部は、
前記全体多段マップを生成する際に、
最新の前記多段マップが追加されると共に、過去の前記多段マップのうちで最古の前記多段マップを削除する
ことを特徴とする請求項１乃至３いずれか１項に記載の３次元環境復元装置。
【請求項５】
前記環境認識センサは、
観測物体までの距離データを取得するレーザレンジファインダである
ことを特徴とする請求項１乃至４いずれか１項に記載の３次元環境復元装置。
【請求項６】
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、当該多段マップを生成及び蓄積する３次元環境復元方法であって、
環境が３次元で計測された計測データを取得する環境認識ステップと、
前記計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成ステップと、
前記３次元点群データを用いて前記多段マップを時分割で生成する時分割多段マップ生成ステップと、
前記多段マップを記憶手段に規定時間分蓄積する時分割多段マップ蓄積ステップと、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成ステップと、
を有する３次元環境復元方法。
【請求項７】
環境を３次元で計測する環境認識センサを備え、当該環境認識センサで計測された計測データに基づいて生成する３次元環境地図を用いて動作を行うロボットであって、
前記環境認識センサで計測された計測データから、観測物体の３次元形状を表す３次元点群を生成する３次元点群生成処理部と、
３次元環境を表現するデータ構造であって、当該３次元環境が格子領域に区切られて前記観測物体が存在する格子領域に直方体が配置された３次元環境マップを多段マップとして、前記３次元点群生成処理部で生成された３次元点群データを用いて前記多段マップを時分割で生成し、当該生成した多段マップを記憶手段に規定時間分蓄積する時分割多段マップ生成／蓄積処理部と、
前記記憶手段に蓄積された前記規定時間分の前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された多段マップと、を合成することで、新たな環境情報を反映させた全体多段マップを生成する全体多段マップ生成処理部と、
を備えるロボット。
【請求項８】
前記時分割多段マップ生成／蓄積処理部は、
前記３次元点群生成処理部で生成された３次元点群データを用いて第１の格子サイズの前記多段マップを時分割で生成し、当該生成した第１の格子サイズの多段マップを記憶手段に規定時間分蓄積し、
前記３次元点群生成処理部で生成された３次元点群データを用いて前記第１の格子サイズよりも大きな第２の格子サイズの前記多段マップを時分割で生成し、当該生成した第２の格子サイズの多段マップを前記記憶手段に規定時間分蓄積し、
前記全体多段マップ生成処理部は、
前記記憶手段に蓄積された前記規定時間分の前記第１の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された記第１の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第１の格子サイズの全体多段マップ生成し、
前記記憶手段に蓄積された前記規定時間分の前記第２の格子サイズの前記多段マップと、前記時分割多段マップ生成／蓄積処理部で新たに生成された前記第２の格子サイズの多段マップと、を合成することで、新たな環境情報を反映させた前記第２の格子サイズの全体多段マップ生成し、
前記ロボットは、前記第１の格子サイズの全体多段マップ及び前記第２の格子サイズの全体多段マップを用途に応じて使い分ける
ことを特徴とする請求項７記載のロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−１０３１４４（Ｐ２０１２−１０３１４４Ａ）
【公開日】平成２４年５月３１日（２０１２．５．３１）
【国際特許分類】
【出願番号】特願２０１０−２５２４８７（Ｐ２０１０−２５２４８７）
【出願日】平成２２年１１月１１日（２０１０．１１．１１）
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年５月３１日（２０１２．５．３１）
【国際特許分類】
【出願日】平成２２年１１月１１日（２０１０．１１．１１）
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]