JP2010066595A - 環境地図生成装置及び環境地図生成方法 - Google Patents

Abstract

【課題】数の３次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することを可能とする。
【解決手段】環境地図を生成する環境地図生成装置１００は、所定の環境内を視覚的に認識する視覚センサ１１と、視覚センサ１１による計測結果を用いて、環境に存在する計測対象の位置を示す３次元位置データを生成する３次元位置データ生成部１２と、異なる計測位置で計測された複数の３次元位置データを統合することにより、環境に関する環境地図を生成する環境地図生成部１３とを備える。環境地図生成部１３は、法線が互いに平行となっていない少なくとも３つの平面であって、複数の３次元位置データにおいて対応する平面を有する平面を用いて、複数の３次元位置データを統合する。
【選択図】図１

Description

本発明は、環境地図生成装置、環境地図生成方法に関し、特に移動ロボットが移動するために必要な環境地図を生成する環境地図生成装置及び環境地図生成方法に関する。

移動ロボットは、環境地図を参照することによって移動経路を計画し、当該移動経路に従って移動を行う。例えば、環境地図は、移動ロボットが移動を行う２次元の移動空間を格子状に分割するとともに、格子に囲まれた各々のセルが、移動ロボットにとって移動可能な領域であるか否かを表したグリッド地図として作成される。

移動ロボットには、環境地図生成装置が搭載されている。また、環境地図生成装置には、レーザレンジファインダなどの視覚センサが搭載されている。そして、環境地図生成装置は、視覚センサにより取得される距離データを用いて、外部環境の３次元位置データを生成する。次に、３次元位置データから複数の平面を検出することにより、環境地図を生成する（例えば、特許文献１〜５参照）。

また、環境地図生成装置は、複数の異なる位置で３次元位置データを計測することによって、広範囲の環境地図を生成することができる。具体的には、環境地図生成装置は、当該複数の３次元位置データを計測位置などに基づいて繋ぎ合わせる。これにより、環境地図生成装置は、複数の局所的な３次元位置データが統合された広範囲の環境地図を生成する。

例えば、ＩＣＰアルゴリズムを用いて、複数の３次元位置データにおいて、一の３次元位置データにおける測定点と、当該測定点に対応する他の３次元位置データにおける測定点との距離を最小化する。これにより、複数の局所的な３次元位置データを繋ぎ合わせる。

また、環境にマーカ（特徴点）を設け、当該マーカを指標として、複数の３次元位置データを繋ぎ合わせる方法もある。
特許第２８１３７６７号公報 特開平９−２９７０１９号公報 特開２００１−２６６１２３号公報 特開２０００−２２１０３７号公報 特開２００３−３１７０８１号公報

しかしながら、上述のＩＣＰアルゴリズムを用いて、複数の３次元位置データにおいて対応する測定点間の距離を最小化することにより、広範囲の環境地図を生成する方法は、点と点とを合わせる方法である。そのため、３次元位置データとしては精度が高くても、個々の点としては精度が低い場合には、複数の３次元位置データを正確に繋ぐことができない。
また、上述のマーカを用いて、複数の３次元位置データを繋ぎ、広範囲の環境地図を生成する方法では、マーカの位置に誤差があると、正確に環境地図を生成することができない。特に、大規模な３次元位置データの繋ぎ合わせとなると、マーカの位置誤差の影響が大きくなってしまう。

本発明は、上述した知見に基づいてなされたものであって、複数の３次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することが可能な環境地図生成装置及び環境地図生成方法を提供することを目的とする。

本発明の第１の態様は、環境地図を生成する環境地図生成装置であり、視覚センサ、３次元位置データ生成部、環境地図生成部を備える。前記視覚センサは、所定の環境内を視覚的に認識する。前記３次元位置データ生成部は、前記視覚センサによる計測結果を用いて、前記環境に存在する計測対象の位置を示す３次元位置データを生成する。前記環境地図生成部は、異なる計測位置で計測された複数の前記３次元位置データを統合することにより、前記環境に関する環境地図を生成する。さらに、前記環境地図生成部は、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有する平面を用いて、複数の前記３次元位置データを統合する。

本発明の第１の態様にかかる環境地図生成装置では、環境地図生成部が、法線が互いに平行となっていない少なくとも３つの平面を用いて、複数の３次元位置データを統合する。そのため、従来のＩＣＰを用いて点と点とを合わせることにより複数の３次元位置データを統合する場合に比べて、複数の３次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、本発明の第１の態様にかかる環境地図生成装置は、より精細な環境地図を生成することができる。

また、前記環境地図生成部は、初期位置合わせ部を備えることが好ましい。前記初期位置合わせ部は、前記環境内の特徴点であるマーカを用いて、複数の前記３次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の３次元位置データを統合する際の計算コストを軽減することができる。

さらに、前記環境地図生成部は、平面検出部を備えることが好ましい。前記平面検出部は、プレーンセグメントファインダと呼ばれる手法を用いて、前記３次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。

さらにまた、前記環境地図生成部は、信頼度算出部を備えることが好ましい。前記信頼度算出部は、前記平面検出部によって検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する。
前記信頼度算出部は、前記平面検出部によって検出された平面に属する前記３次元位置データの点の数に基づいて、前記スコアを算出してもよい。
また、前記信頼度算出部は、前記平面検出部によって検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出してもよい。
信頼度算出部によって、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成部が複数の３次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。

また、前記環境地図生成部は、剛体変換パラメータ算出部と地図統合部とを備える。前記剛体変換パラメータ算出部は、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する。また、前記地図統合部は、前記剛体変換パラメータ算出部によって算出された剛体変換パラメータを用いて、複数の前記３次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記３次元位置データを統合する。
剛体変換パラメータ算出部は、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出部は、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合部は、複数の３次元位置データをより精度良く統合することができる。したがって、環境地図生成装置は、より精細な環境地図を生成することができる。

本発明の第２の態様は、環境地図を生成する環境地図生成方法であって、３次元位置データ生成ステップと、環境地図生成ステップとを備える。前記３次元位置データ生成ステップでは、視覚センサによる計測結果を用いて、所定の環境に存在する計測対象の位置を示す３次元位置データを生成する。また、前記環境地図生成ステップでは、異なる計測位置で計測された複数の前記３次元位置データを統合することにより、前記環境に関する環境地図を生成する。さらに、前記環境地図生成ステップでは、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有する平面を用いて、複数の前記３次元位置データを統合する。

本発明の第２の態様にかかる環境地図生成方法では、環境地図生成ステップにおいて、法線が互いに平行となっていない少なくとも３つの平面を用いて、複数の３次元位置データを統合する。そのため、従来のＩＣＰを用いて点と点とを合わせることにより複数の３次元位置データを統合する場合に比べて、複数の３次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、本発明の第２の態様にかかる環境地図生成方法は、より精細な環境地図を生成することができる。

また、前記環境地図生成ステップは、初期位置合わせステップを備えることが好ましい。前記初期位置合わせステップでは、前記環境内の特徴点であるマーカを用いて、複数の前記３次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の３次元位置データを統合する際の計算コストを軽減することができる。

さらに、前記環境地図生成ステップは、平面検出ステップを備えることが好ましい。前記平面検出ステップでは、プレーンセグメントファインダと呼ばれる手法を用いて、前記３次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。

さらにまた、前記環境地図生成ステップは、信頼度算出ステップを備えることが好ましい。前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する。
前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面に属する前記３次元位置データの点の数に基づいて、前記スコアを算出してもよい。
また、前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出してもよい。
信頼度算出ステップにおいて、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成ステップにおいて、複数の３次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。

また、前記環境地図生成ステップは、剛体変換パラメータ算出ステップと地図統合ステップとを備えることが好ましい。前記剛体変換パラメータ算出ステップでは、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する。また、地図統合ステップでは、前記剛体変換パラメータ算出ステップにおいて算出された剛体変換パラメータを用いて、複数の前記３次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記３次元位置データを統合する。
剛体変換パラメータ算出ステップでは、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出ステップでは、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合ステップでは、複数の３次元位置データをより精度良く統合することができる。したがって、環境地図生成方法は、より精細な環境地図を生成することができる。

本発明により、複数の３次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することができる。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。

＜発明の実施の形態１＞
本実施の形態にかかる環境地図生成装置１００は、複数の計測位置において、環境地図生成装置１００の外界の距離画像データを取得する。即ち、環境地図生成装置１００は、所定の環境内の異なる範囲の距離画像データを複数取得する。以下、一つの計測位置から計測を行う範囲をフレームと称する。次いで、環境地図生成装置１００は、当該距離画像データから３次元位置データを生成する。即ち、環境地図生成装置１００は、各フレーム毎に３次元位置データを生成する。次いで、環境地図生成装置１００は、各フレーム毎に３次元位置データから平面を検出する。そして、環境地図生成装置１００は、一のフレームの平面と対応する他のフレームの平面とを利用して、当該一のフレームと他のフレームを繋ぎ合わせる。これにより、環境地図生成装置１００は、複数のフレームを繋ぎ合わせ、広範囲の環境地図を生成する。
環境地図生成装置１００は、例えば、様々な環境下において動作するロボットや車両などに搭載される。そして、当該ロボットや車両は、環境地図生成装置１００により生成された環境地図を参照し、移動経路を計画し、当該移動経路に従って移動を行う。

図１に、環境地図生成装置１００の制御系の構成を示す。図１に示すように、環境地図生成装置１００は、視覚センサ１１、３次元位置データ生成部１２、地図生成部１３（環境地図生成部）、環境地図記憶部１４などを備えている。

視覚センサ１１は、レーザレンジファインダ等のアクティブ距離センサを有している。そして、視覚センサ１１は、環境地図生成装置１００の環境の所定範囲の距離画像データを取得する。視覚センサ１１が計測を行う範囲は、視覚センサ１１の性能によって規定される。そして、視覚センサ１１が計測を行う範囲が、各フレームの大きさを規定する。また、環境地図生成装置１００が異なる複数の計測位置において計測を行うことにより、視覚センサ１１は、異なる範囲の距離画像データを取得する。

なお、環境地図生成装置１００は、視覚センサ１１として、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備えてもよい。そして、視覚センサ１１は、これら複数のカメラによって撮影した画像データを用いて距離画像データを生成してもよい。具体的には、視覚センサ１１は、複数のカメラによって撮影した画像データから対応点を検出する。次いで、視覚センサ１１は、ステレオ視によって対応点の３次元位置を復元すればよい。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

３次元位置データ生成部１２は、距離画像データの座標変換を行って３次元位置データを生成する。なお、３次元位置データとは、距離画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。ここで、３次元位置データに含まれる点群の座標は、環境地図生成装置１００に固定された装置座標系で表示されていてもよいし、環境に固定されたグローバル座標系で表示されていてもよい。

地図生成部１３は、複数の計測位置で得られた局所的な３次元位置データを統合することにより、広範囲の環境地図を生成する。即ち、複数のフレームの３次元位置データを繋ぎ合わせることにより、広範囲の環境地図を生成する。本実施の形態では、環境地図が、３次元地図であるとして説明する。
具体的には、地図生成部１３は、法線が互いに平行となっていない少なくとも３つの平面であって、複数のフレームの３次元位置データにおいて対応する平面を有し、且つ、所定値以上のスコア（後述）を有する平面を用いて、複数のフレームの３次元位置データを統合する。
地図生成部１３により生成された環境地図は、環境地図記憶部１４に格納される。

図２を参照しながら、地図生成部１３による環境地図の生成方法を説明する。図２（ａ）は、環境地図生成装置１００が計測を行う環境の一例を示している。図２（ａ）の環境１６０は、床面１６１上に、直方体形状の障害物１６２と円柱形状の障害物１６３を有している。そして、環境１６０は、床面１６１に相当する平面Ａと障害物１６２の前面に相当する平面Ｂ、障害物１６２の紙面向かって右側の側面に相当する平面Ｃ、障害物１６２の上面に相当する平面Ｄ、障害物１６３の上面に相当する平面Ｅを有している。
また、図２（ａ）に示すように、環境地図生成装置１００は、一の計測位置Ａと、計測位置Ａとは異なる他の計測位置Ｂから、環境１６０の計測を行う。また、図２（ａ）において、計測地点Ａ及び計測地点Ｂは、環境１６０に対して同じ側となっている。

図２（ｂ）に、計測位置Ａから計測を行った場合に得られる３次元地図を示す。また、図２（ｃ）に、計測位置Ｂから計測を行った場合に得られる３次元地図を示す。図２（ａ）〜図２（ｄ）において、計測地点Ａ及び計測地点Ｂから奥行き方向をｘ軸、計測地点Ａ及び計測地点Ｂの左右方向をｙ軸、計測地点Ａ及び計測地点Ｂの上下方向をｚ軸とする。
図２（ｂ）に示すように、計測位置Ａから計測を行って得られる３次元地図には、床面１６１に相当する平面Ａの一部と、障害物１６２に相当する平面Ｂ、平面Ｃ、平面Ｄとが表わされている。また、図２（ｃ）に示すように、計測位置Ｂから計測を行って得られる３次元地図には、床面１６１に相当する平面Ａの一部と、障害物１６２に相当する平面Ｂ及び平面Ｄの一部、平面Ｃと、障害物１６２に相当する平面Ｅとが表わされている。したがって、図２（ｂ）に示す３次元地図と、図２（ｃ）に示す３次元地図とは、一部重複している。

そして、地図生成部１３は、重複部分において、図２（ｂ）に示す３次元地図と図２（ｃ）に示す３次元地図とを重ね合わせることにより、広範囲の環境地図を生成する。図２（ｄ）に、図２（ｂ）に示す３次元地図と図２（ｃ）に示す３次元地図とを繋ぎ合わせることにより生成される環境地図を示す。また、図２（ｄ）において、図２（ｂ）の３次元地図と図２（ｃ）の３次元地図とを繋ぎ合わせた部分における図２（ｂ）の３次元地図の境界線を破線で示し、図２（ｃ）の３次元地図の境界線を一点鎖線で示す。
具体的には、地図生成部１３は、図２（ｂ）に含まれる平面と、当該平面に対応する図２（ｃ）に含まれる平面とを利用して、図２（ｂ）の３次元地図と図２（ｃ）の３次元地図とを繋ぎ合わせる。

以下、地図生成部１３の構成及び環境地図生成方法について、詳細に説明する。
図１に示すように、地図生成部１３は、初期位置合わせ部１３１、平面検出部１３２、信頼度算出部１３３、対応平面検出部１３４、剛体変換パラメータ算出部１３５、地図統合部１３６を含む。

初期位置合わせ部１３１は、マーカを指標として、複数のフレームの３次元位置データの位置合わせを行う。ここで、マーカとは、環境内の特徴点である。そして、初期位置合わせ部１３１は、一のフレーム内に含まれるマーカと、当該マーカに対応する他のフレーム内に含まれるマーカとの位置合わせを行うことにより、当該２つのフレームの初期位置合わせを行う。このようにして、初期位置合わせ部１３１は、複数のフレーム内の互いに対応するマーカの位置が合うように、複数のフレームの初期位置合わせを行う。

平面検出部１３２は、各フレーム毎に、３次元位置データから平面を検出する。また、平面検出部１３２は、検出した平面に平面ＩＤを付与する。本実施の形態における平面検出には、プレーンセグメントファインダ（Ｐｌａｎｅ Ｓｅｇｍｅｎｔ Ｆｉｎｄｅｒ（ＰＬＳ））と呼ばれる手法が用いられる。プレーンセグメントファインダでは、まず、３次元の点群から２点をランダムにサンプリングする。次いで、当該２点のベクトルをクラスタリングすることにより、平面の位置姿勢を求めるアルゴリズムである。
具体的には、プレーンセグメントファインダでは、まず、３次元位置データに対してランダマイズド３次元ハフ変換を行って、最大平面の傾きを検出する。次に、再び、ランダマイズド３次元ハフ変換を用いて、当該最大平面と原点との距離を検出する。そして、当該最大平面に属する点を抽出することにより、平面領域（プレーンセグメント）を検出する。

より具体的には、プレーンセグメントファインダにおいて、平面は、以下の数式（１）により表わされる。
ρ＝（ｘ_０ｃｏｓ（θ）＋ｙ_０ｓｉｎ（θ））ｃｏｓ（φ）＋ｚ_０ｓｉｎ（φ）
・・・・・・（１）
数式（１）において、ρは平面から原点へ下ろした垂線の長さ、θは当該垂線をｘｙ平面に投影した線とｘ軸とのなす角、φは当該垂線とｘｙ平面とのなす角である。
そして、３次元位置データ内の各測定点（ｘ_０,ｙ_０,ｚ_０）を（ρ,θ,φ）空間に投影し、ピークを検出することにより、平面を検出する。プレーンセグメントファインダでは、複数に分割された平面であっても検出することができる。また、３次元位置データの所定部分の測定点のみを（ρ,θ,φ）空間の部分空間に投影して、検出範囲を狭めることができる。これにより、計算コストを大幅に低減することも可能である。

なお、多数の計測点（３次元位置データ）から平面の方程式を表す平面パラメータ（例えば、法線ベクトル及び法線ベクトルの座標系原点からの距離）を検出するために、ハフ変換法やランダムサンプリング法が従来から用いられている。本実施の形態における平面検出に、従来から知られているこれらの手法を適用してもよい。

信頼度算出部１３３は、平面検出部１３２によって検出された各平面の信頼度を評価し、信頼度を表すスコアを算出し、各平面に当該スコアを付与する。
平面の面積が大きいほど角度精度が上がるため、各平面の信頼度は高くなる。そこで、平面のスコアは、検出された平面に属するデータの点数に基づいて評価する。
また、計測方向に対して垂直の角度にある平面は計測精度が高くなる。そこで、平面のスコアは、視覚センサ１１の位置と平面位置（平面データの重心）とを結ぶ直線と、当該平面の法線との内積に基づいて評価する。

対応平面検出部１３４は、ＩＣＰ（Ｉｎｔｅｒａｃｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）アルゴリズムを用いて、一のフレームに含まれる平面と対応する他のフレームに含まれる平面を検出する。
図３を用いて、対応平面検出部１３４による処理を説明する。図３に示すように、対応平面検出部１３４は、一のフレーム内の平面Ａに含まれる点（図３において白抜きの丸で示す）から最も近傍にある他のフレーム内の点（図３においてハッチングが付された丸で示す）に着目する。そして、平面検出部１３３は、当該他のフレーム内の点が属する平面の平面ＩＤを検出する。そして、対応平面検出部１３４は、一のフレーム内の平面Ａに属する全ての点から最も近傍にある他のフレーム内の点が属する平面の平面ＩＤとして最も多く検出された平面ＩＤを有する平面（図３の例では平面Ａ'）を平面Ａの対応する平面として検出する。同様に、対応平面検出部１３４は、一のフレーム内の平面Ｂに属する全ての点から最も近傍にある他のフレーム内の点が属する平面ＩＤとして最も多く検出された平面ＩＤを有する平面（図３の例では平面Ｂ'）を平面Ｂの対応する平面として検出する。

剛体変換パラメータ算出部１３５は、対応平面検出部１３４によって検出された対応する平面を用いて剛体変換パラメータを求める。
具体的には、剛体変換パラメータ算出部１３５は、対応平面検出部１３４によって他のフレーム内に対応する平面が検出された平面であり、信頼度算出部１３３によって算出されたスコアが高い平面を少なくとも３つ選択する。また、選択された各平面の法線は互いに平行となっていない。
そして、剛体変換パラメータ算出部１３５は、一のフレームから選択した少なくとも３つの平面と、他のフレーム内の当該平面と対応する少なくとも３つの平面とをフィッティングする。これにより、剛体変換パラメータを算出する。
ここで、剛体変換パラメータとは、剛体変換式において用いられるパラメータである。剛体変換式は、以下の数式（２）により表わされる。
Ｄ_ｔ−１＝Ｒ×Ｄ_ｔ＋Ｔ ・・・・・・（２）
数式（２）において、Ｒは回転を表わすパラメータであり、Ｔは並進を表わすパラメータである。説明の簡単のため、剛体変換パラメータ算出部１３５が図２（ｂ）及び図２（ｃ）において、平面Ａ、平面Ｂ、平面Ｃを選択したとする。また、図４に示すように、図２（ｂ）、図２（ｃ）の何れか一方の平面Ａ、平面Ｂ、平面Ｃがそれぞれ、座標系のｘｙ平面上、ｘｚ平面上、ｚｙ平面上にあるとする。また、ｘ軸を回動軸とする回動をｒｏｌｌ、ｙ軸を回動軸とする回動をｐｉｔｃｈ、ｚ軸を回動軸とする回動をｙａｗと表わす。
このとき、Ｒは回転を表わすパラメータであるため、ｒｏｌｌ、ｐｉｔｃｈ、ｙａｗで表わされる。また、Ｔは並進を表わすパラメータであるため、ｘ、ｙ、ｚで表わされる。

そして、図２（ｂ）の平面Ａと図２（ｃ）の平面Ａとをフィッティングすることにより、ｚ、ｒｏｌｌ、ｐｉｔｃｈを一意に求めることができる。また、図２（ｂ）の平面Ｂと図２（ｃ）の平面Ｂとをフィッティングすることにより、ｙａｗ、ｙを一意に求めることができる。また、図２（ｂ）の平面Ｃと図２（ｃ）の平面Ｃとをフィッティングすることにより、ｘを一意に求めることができる。
したがって、剛体変換パラメータ算出部１３５は、法線が互いに平行となっていない少なくとも３組の平面（図２（ｂ）の平面Ａと図２（ｃ）の平面Ａ、図２（ｂ）の平面Ｂと図２（ｃ）の平面Ｂ、図２（ｂ）の平面Ｃと図２（ｃ）の平面Ｃ）を用いて剛体変換パラメータを算出することができる。

また、剛体変換パラメータ算出部１３５は、３フレーム以上の３次元位置データにおいて互いに対応する平面を用いて剛体変換パラメータＲ及びＴを算出する場合には、全ての平面において、各平面を表す平面式（例えば、数式（１）の平面式）の一致誤差が最小となるような剛体変換パラメータを算出する。

地図統合部１３６は、異なる計測位置で得られた複数の３次元位置データを統合することにより、広範囲の環境地図を生成する。即ち、地図統合部１３６は、複数のフレームの３次元位置データを繋ぎ合わせて、広範囲の環境地図を生成する。例えば、地図統合部１３６は、図２（ｂ）に示す３次元地図と図２（ｃ）に示す３次元地図とを重ね合わせることにより、広範囲の環境地図（図２（ｄ））を生成する。
具体的には、地図統合部１３６は、剛体変換パラメータ算出部１３５によって算出された剛体変換パラメータＲ及びＴと数式（２）を用いて、各フレームの座標系を変換して統一する。これにより、地図統合部１３６は、複数のフレームを繋ぎ合わせて広範囲の環境地図を生成する。

続いて以下では、環境地図生成装置１００の環境地図生成方法について図５を参照して詳細に説明する。図５は、環境地図生成装置１００による環境地図の生成手順の具体例を示すフローチャートである。
まず、環境地図生成装置１００は、複数の計測地点において計測を行って、複数フレームの距離画像データを取得する。次いで、３次元位置データ生成部１２は、複数フレームの距離画像データの座標変換を行って、複数フレームの３次元位置データを生成する（ステップＳ１；３次元位置データ生成ステップ）。

次に、初期位置合わせ部１３１は、マーカを指標として、複数のフレームの３次元位置データの位置合わせを行う（ステップＳ２；初期位置合わせステップ）。

次に、平面検出部１３２は、各フレーム毎に、３次元位置データから平面を検出する（ステップＳ３；平面検出ステップ）。また、平面検出部１３２は、検出した平面に平面ＩＤを付与する。

次に、信頼度算出部１３３は、平面検出部１３２によって検出された各平面の信頼度を評価し、スコアを算出し、各平面に当該スコアを付与する（ステップＳ４；信頼度算出ステップ）。

次に、対応平面検出部１３４は、ＩＣＰアルゴリズムを用いて、一のフレームに含まれる平面と対応する他のフレームに含まれる平面を検出する（ステップＳ５）。

次に、剛体変換パラメータ算出部１３５は、一のフレームから少なくとも３つの平面を選択する。剛体変換パラメータ算出部１３５によって選択される平面は、他のフレーム内に対応する平面が検出されており、スコアが高いものである。また、剛体変換パラメータ算出部１３５によって選択される各平面の法線は互いに平行となっていない。そして、剛体変換パラメータ算出部１３５は、一のフレームから選択した少なくとも３つの平面と、他のフレーム内の当該平面と対応する少なくとも３つの平面とをフィッティングする。これにより、剛体変換パラメータを算出する（ステップＳ６；剛体変換パラメータ算出ステップ）。

次に、地図統合部１３６は、異なる計測位置で得られた複数フレームの３次元位置データを繋ぎ合わせて広範囲の環境地図を生成する（ステップＳ７；地図統合ステップ）。具体的には、地図統合部１３６は、剛体変換パラメータ算出部１３５によって算出された剛体変換パラメータＲ及びＴと数式（２）を用いて、各フレームの座標系を変換して統一する。これにより、地図統合部１３６は、複数のフレームを繋ぎ合わせて広範囲の環境地図を生成する。

このようにして、本実施の形態にかかる環境地図生成装置１００によって生成された環境地図の一例を図６、図７に示す。
図６、図７は、東大安田講堂周辺の環境地図である。１８箇所の計測位置において、合計約８０回、計測を行った。また、視覚センサ１１の解像度は、０．１°であり、計測範囲は、上下８０°、左右３６０°である。
図６は、東大安田講堂周辺を上側から表す環境地図である。具体的には、図６に示す環境地図は、ｘｙ平面上で表され、それぞれの点がｚ軸方向（高さ方向）の高さについてのデータを保持している。
図７は、安田講堂周辺を表す３次元の環境地図であり、いわゆる俯瞰図である。

以上に説明したように、本実施の形態にかかる環境地図生成装置１００においては、地図生成部１３が、法線が互いに平行となっていない少なくとも３つの平面を用いて、複数の３次元位置データを統合する。そのため、従来のＩＣＰを用いて点と点とを合わせることにより複数の３次元位置データを統合する場合に比べて、複数の３次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、環境地図生成装置１００は、より精細な環境地図を生成することができる。

また、初期位置合わせ部１３１は、環境内の特徴点であるマーカを用いて、複数の３次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の３次元位置データを統合する際の計算コストを軽減することができる。

さらに、平面検出部１３２は、プレーンセグメントファインダと呼ばれる手法を用いて、３次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。

さらにまた、信頼度算出部１３３によって、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成部１３が複数の３次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。

また、剛体変換パラメータ算出部１３５は、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出部１３５は、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合部１３６は、複数の３次元位置データをより精度良く統合することができる。したがって、環境地図生成装置１００は、より精細な環境地図を生成することができる。

なお、環境地図生成装置１００は、脚歩行型の移動ロボットに適用してもよい。また、環境地図生成装置１００は、車輪走行型などの脚歩行型以外の移動ロボットに適用されてもよい。

また、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

発明の実施の形態１にかかる環境地図生成装置の制御系を示すブロック図である。 発明の実施の形態１にかかる環境地図生成装置が計測する環境の一例及び環境地図の一例を示す図である。 発明の実施の形態１にかかる対応平面検出部による処理を説明する図である。 発明の実施の形態１にかかる剛体変換パラメータ算出部による処理を説明する図である。 発明の実施の形態１にかかる環境地図生成装置の環境地図生成方法を示すフローチャートである。 発明の実施の形態１にかかる環境地図生成装置によって生成される環境地図の一例を示す図である。 発明の実施の形態１にかかる環境地図生成装置によって生成される環境地図の一例を示す図である。

符号の説明

１００ 環境地図生成装置
１１ 視覚センサ
１２ ３次元位置データ生成部
１３ 地図生成部（環境地図生成部）
１３１ 初期位置合わせ部
１３２ 平面検出部
１３３ 信頼度算出部
１３４ 対応平面検出部
１３５ 剛体変換パラメータ算出部
１３６ 地図統合部

Claims (14)

1. 環境地図を生成する環境地図生成装置であって、
   所定の環境内を視覚的に認識する視覚センサと、
   前記視覚センサによる計測結果を用いて、前記環境に存在する計測対象の位置を示す３次元位置データを生成する３次元位置データ生成部と、
   異なる計測位置で計測された複数の前記３次元位置データを統合することにより、前記環境に関する環境地図を生成する環境地図生成部とを備え、
   前記環境地図生成部は、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有する平面を用いて、複数の前記３次元位置データを統合する環境地図生成装置。
2. 前記環境地図生成部は、
   前記環境内の特徴点であるマーカを用いて、複数の前記３次元位置データ同士の位置を合わせる初期位置合わせ部を備える請求項１に記載の環境地図生成装置。
3. 前記環境地図生成部は、
   プレーンセグメントファインダと呼ばれる手法を用いて、前記３次元位置データから平面を検出する平面検出部を備える請求項１又は２に記載の環境地図生成装置。
4. 前記環境地図生成部は、
   前記平面検出部によって検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する信頼度算出部を備える請求項３に記載の環境地図生成装置。
5. 前記信頼度算出部は、前記平面検出部によって検出された平面に属する前記３次元位置データの点の数に基づいて、前記スコアを算出する請求項４に記載の環境地図生成装置。
6. 前記信頼度算出部は、前記平面検出部によって検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出する請求項４又は５に記載の環境地図生成装置。
7. 前記環境地図生成部は、
   法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する剛体変換パラメータ算出部と、
   前記剛体変換パラメータ算出部によって算出された剛体変換パラメータを用いて、複数の前記３次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記３次元位置データを統合する地図統合部とを備える請求項４乃至６の何れか一項に記載の環境地図生成装置。
8. 環境地図を生成する環境地図生成方法であって、
   視覚センサによる計測結果を用いて、所定の環境内に存在する計測対象の位置を示す３次元位置データを生成する３次元位置データ生成ステップと、
   異なる計測位置で計測された複数の前記３次元位置データを統合することにより、前記環境に関する環境地図を生成する環境地図生成ステップとを備え、
   前記環境地図生成ステップでは、法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有する平面を用いて、複数の前記３次元位置データを統合する環境地図生成方法。
9. 前記環境地図生成ステップは、
   前記環境内の特徴点であるマーカを用いて、複数の前記３次元位置データ同士の位置を合わせる初期位置合わせステップを備える請求項８に記載の環境地図生成方法。
10. 前記環境地図生成ステップは、
    プレーンセグメントファインダと呼ばれる手法を用いて、前記３次元位置データから平面を検出する平面検出ステップを備える請求項８又は９に記載の環境地図生成方法。
11. 前記環境地図生成ステップは、
    前記平面検出ステップにおいて検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する信頼度算出ステップを備える請求項１０に記載の環境地図生成方法。
12. 前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面に属する前記３次元位置データの点の数に基づいて、前記スコアを算出する請求項１１に記載の環境地図生成方法。
13. 前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出する請求項１１又は１２に記載の環境地図生成方法。
14. 前記環境地図生成ステップは、
    法線が互いに平行となっていない少なくとも３つの平面であって、複数の前記３次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する剛体変換パラメータ算出ステップと、
    前記剛体変換パラメータ算出ステップにおいて算出された剛体変換パラメータを用いて、複数の前記３次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記３次元位置データを統合する地図統合ステップとを備える請求項１１乃至１３の何れか一項に記載の環境地図生成方法。