JPWO2021111613A1 - ３次元地図作成装置、３次元地図作成方法、及び３次元地図作成プログラム - Google Patents

Abstract

３次元地図作成装置（１００）は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成する３次元地図生成部（１１０）と、フロアのフロアマップ（３００）を取得し、３次元地図生成部（１１０）によって生成された１つ以上の第１の３次元地図をフロアマップ（３００）上に配置することによって、１つ以上の第１の３次元地図を含む第２の３次元地図を生成するフロアマップ登録部（１２０）とを有する。

Description

本発明は、３次元地図作成装置、３次元地図作成方法、及び３次元地図作成プログラムに関する。

工場又はビルなどの大規模な屋内環境において自己位置を検出するためのシステムとして、ＷｉＦｉ又はＢｅａｃｏｎを利用した測位システムが知られている。しかし、例えば、Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ（ＡＧＶ）に搭載されたロボットである自律移動ロボットでは、点検の対象物の位置に加えて姿勢を検出したいという要求がある。また、対象物の位置及び姿勢を検出する装置では、導入コストの観点から位置及び姿勢を検出するための追加の機器をできるだけ無くしたいという要求がある。これらの要求を満たす方法として、自律移動ロボットの用途及び実在する風景にバーチャルの視覚情報としてのコンテンツを重ねて表示する拡張現実の用途などにおいて３次元地図を用いる方法が知られている。

３次元地図を作成する方法としては、Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ（ＬｉＤＡＲ）、カメラ、などのセンサから取得されたセンサデータに基づいて、自己位置の推定と地図作成とを同時に行うＳｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ（ＳＬＡＭ）が知られている。

特開２０１４−２２９０２０号公報

しかしながら、ＳＬＡＭでは移動距離に応じて位置誤差が蓄積されるため、ＳＬＡＭを大規模な屋内環境で使用した場合、精度の高い３次元地図を作成できないという問題がある。

ここで、図１（Ａ）及び（Ｂ）は、ＳＬＡＭによる３次元地図作成の対象領域であるフロア２０１及びフロア２０１の壁面に沿って移動しながら屋内環境をスキャンした場合における位置誤差の蓄積の様子であるスキャン結果２０２をそれぞれ示す。また、図２（Ａ）及び（Ｂ）は、実在する対象物（例えば、機器）Ａ１〜Ａ３の画像２１１にコンテンツ２１２を正常に重ねて表示するタブレットＰＣ（Ｔａｂｌｅｔ Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）２１０及び非正常に重ねて表示するタブレットＰＣ２１０をそれぞれ示す。

本発明は、上記課題を解決するためになされたものであり、精度が高い３次元地図を生成することができる３次元地図作成装置、３次元地図作成方法、及び３次元地図作成プログラムを提供することを目的とする。

本発明の一態様に係る３次元地図作成装置は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成する３次元地図生成部と、前記フロアのフロアマップを取得し、前記３次元地図生成部によって生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記１つ以上の第１の３次元地図を含む第２の３次元地図を生成するフロアマップ登録部とを有する。

本発明の他の態様に係る３次元地図作成方法は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成するステップと、前記フロアのフロアマップを取得し、生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記１つ以上の第１の３次元地図を含む第２の３次元地図を生成するステップとを有する。

本発明によれば、精度が高い３次元地図を生成することができる。

（Ａ）は、ＳＬＡＭによる３次元地図作成の対象領域である現実の１フロアを示す図であり、（Ｂ）は、１フロアの壁面に沿って移動しながら屋内環境をスキャンした場合における位置誤差の蓄積の様子であるスキャン結果を示す図である。 （Ａ）は、実在する対象物を表示する画像にバーチャルな視覚情報としてのコンテンツを正常に重ねて表示するタブレットＰＣを示す図であり、（Ｂ）は、実在する対象物を表示する画像にコンテンツを非正常に重ねて表示するタブレットＰＣを示す図である。 本発明の実施の形態１に係る３次元地図作成装置のハードウェア構成の例を示す図である。 実施の形態１に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。 （Ａ）は、フロアマップの例を示す平面図であり、（Ｂ）は、小規模な第１の３次元地図で表された対象物の例を示す斜視図である。 （Ａ）は、第１の３次元地図のｘｙｚ軸周りの回転を示す斜視図であり、（Ｂ）は、第１の３次元地図の地面の法線周りの回転を示す平面図である。 図４に示されるフロアマップ登録部の構成を概略的に示す機能ブロック図である。 ｘｙｚ直交座標系におけるフロアマップの例を示す図である。 図４に示される範囲分割部によって実行される範囲分割処理を示す図である。 図４に示される範囲分割部の構成を概略的に示す機能ブロック図である。 実施の形態１に係る３次元地図作成装置の範囲指定部を用いた範囲指定操作時の画面を示す図である。 （Ａ）は、３次元地図生成処理を示すフローチャートであり、（Ｂ）は、位置姿勢推定処理を示すフローチャートである。 実施の形態１に係る３次元地図作成装置のフロアマップ登録部によって実行されるフロアマップ登録処理を示すフローチャートである。 実施の形態１に係る３次元地図作成装置の範囲分割部によって実行される範囲分割処理を示すフローチャートである。 類似度を計算する場合と計算しない場合とを説明するための図である。 実施の形態１に係る３次元地図作成装置の範囲分割部によって実行される類似判定処理を示すフローチャートである。 本発明の実施の形態２に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。 （Ａ）は、３次元地図生成処理を示すフローチャートであり、（Ｂ）は、コンテンツ重畳表示のための処理を示すフローチャートである。 本発明の実施の形態２に係る３次元地図作成装置の構成を概略的に示す機能ブロック図である。 （Ａ）は、３次元地図生成処理を示すフローチャートであり、（Ｂ）は、位置姿勢推定処理を示すフローチャートである。

以下に、本発明の実施の形態に係る３次元地図作成装置、３次元地図作成方法、及び３次元地図作成プログラムを、図面を参照しながら説明する。以下の実施の形態は、例にすぎず、本発明の範囲内で種々の変更が可能である。

実施の形態に係る３次元地図作成装置は、例えば、コンピュータを有する自律移動ロボットである。実施の形態に係る３次元地図作成装置は、実在する対象物を表示する画像に視覚情報であるコンテンツを重ねて表示するタブレットＰＣであってもよい。ただし、実施の形態に係る３次元地図作成装置は、ユーザが持ち運ぶことにより移動可能なパーソナルコンピュータ又はスマートフォンなどであってもよい。

本出願において、フロアマップ又はフロア上の物体である対象物（例えば、設備、機器など）を示す図には、発明の理解を容易にするために、ｘｙｚ直交座標系の座標軸と、各座標軸周りの回転方向とが示されている。フロアは、基準平面の例であり、一般的には地面に平行である。ｘ軸及びｚ軸は、フロアを含む平面に平行な座標軸である。ｙ軸は、フロアを含む平面に直交する方向の座標軸である。＋Ｒｚ方向は、＋ｚ軸方向を向いたときにおける時計回り方向であり、−Ｒｚ方向は、＋Ｒｚ方向の逆方向である反時計回り方向である。＋Ｒｘ方向は、＋ｘ軸方向を向いたときにおける時計回り方向であり、−Ｒｘ方向は、＋Ｒｘ方向の逆方向である反時計回り方向である。＋Ｒｙ方向は、＋ｙ軸方向を向いたときにおける時計回り方向であり、−Ｒｙ方向は、＋Ｒｙ方向の逆方向である反時計回り方向である。

《１》実施の形態１
《１−１》構成
〈３次元地図作成装置１００〉
図３は、実施の形態１に係る３次元地図作成装置１００のハードウェア（ＨＷ）構成の例を示す図である。３次元地図作成装置１００は、実施の形態１に係る３次元地図作成方法を実施することができる装置である。３次元地図作成装置１００は、実在する対象物を表示する画像に視覚情報であるコンテンツを重ねて表示するタブレットＰＣであってもよい。実在する対象物は、例えば、機器又は設備などである。

図３に示されるように、３次元地図作成装置１００は、コンピュータ１０を有する。コンピュータ１０は、ソフトウェアとしてのプログラムを格納することができる記憶部としてのメモリ１２と、メモリ１２に記憶されているプログラムを実行することができる情報処理部としてのプロセッサ１１とを有する。プログラムは、実施の形態１に係る３次元地図作成方法をコンピュータ１０に実行させることができる３次元地図作成プログラムを含む。プログラムは、コンピュータ１０によって読み取り可能な記録媒体に記録されることができる。記録媒体は、例えば、磁気ディスク、光ディスク、又は半導体メモリ、などである。

３次元地図作成装置１００は、３次元地図ＤＢ４０を有する。３次元地図ＤＢ４０は、３次元地図を管理するために用いられるデータベース（ＤＢ）が格納された記憶装置である。ただし、３次元地図ＤＢ４０は、３次元地図作成装置１００と通信可能に接続された外部の記憶装置又はネットワーク上のサーバに備えられてもよい。

３次元地図作成装置１００は、距離センサ２１、カメラ２２、ジャイロセンサ２３、加速度センサ２４、地磁気センサ２５などの各種センサのうちの１つ以上を有してもよい。距離センサ２１は、ＬｉＤＡＲ、赤外線などを用いて距離を測定するセンサである。カメラ２２は、画像（例えば、カラー画像）を取得するセンサである。ジャイロセンサ２３は、角速度を取得するセンサである。加速度センサ２４は、加速度を取得するセンサである。地磁気センサ２５は、方位を取得するセンサである。各種センサは、３次元地図作成装置１００の一部であってもよい。ただし、各種センサは、３次元地図作成装置１００と通信可能に接続された外部の装置に備えられてもよい。例えば、図３に示される各種センサ２１〜２５がＡＧＶ上に備えられ、３次元地図作成装置１００が、ＡＧＶ上ではない他の場所に置かれたコンピュータ１０で構成されることも可能である。

また、３次元地図作成装置１００は、ディスプレイ３０を有する。ディスプレイ３０は、画像を表示する表示装置である。３次元地図作成装置１００が、ユーザが携帯するタブレットＰＣである場合、ディスプレイ３０に、図２（Ａ）に示されるような現実の対象物を表示する画像と拡張現実のコンテンツとが表示される。ただし、３次元地図作成装置１００は、ディスプレイ３０を備えない装置であってもよい。

例えば、拡張現実を用いて対象物の保守点検を行う場合、３次元地図作成装置１００であるタブレットＰＣを持ったユーザが対象物の正面まで移動する。３次元地図作成装置１００は、対象物までの移動中において及び対象物の正面位置において取得されたセンサデータに基づいて、対象物の位置及び姿勢の推定を行う。なお、タブレットＰＣの場合には、対象物の位置は、ユーザの位置である自己位置と同じであるものとみなす。拡張現実を用いて対象物の保守点検を行う場合、対象物及びその周辺（すなわち、近くの領域）の３次元地図が正確に作成されていれば、対象物から遠い位置の３次元地図が作成されていなくても或いは不正確であっても、対象物に関するコンテンツを、タブレットＰＣのディスプレイ３０において、対象物の画像上又は対象物の画像の近くの適切な位置に表示することができる。

また、３次元地図作成装置１００である自律移動ロボットを用いて対象物の保守点検を行う場合、自律移動ロボットが対象物の正面まで移動する。自律移動ロボットは、対象物までの移動中における及び対象物の正面位置におけるセンサデータに基づいて、対象物の位置及び姿勢の推定を行う。自律移動ロボットが対象物の保守点検を行う場合、対象物及びその近くの３次元地図が正確に作成されていれば、対象物から遠い位置の３次元地図が作成されていなくても或いは不正確であっても、ロボットハンドなどによって対象物を操作することができる。つまり、自律移動ロボットは、対象物の周辺では、高い位置精度を持つ３次元地図を必要とするが、移動に使用される通路などでは、３次元地図の位置精度は低くても問題ない。

機器単体又は数メートル四方の領域などのような小規模な環境であれば、ＳＬＡＭを用いることによって、高精度に３次元地図を作成することができる。これは、ＳＬＡＭを用いて小規模な環境の３次元地図を作成する場合、誤差の蓄積が少なく、さらに、ループの検出が容易だからである。ループの検出は、例えば、ＳＬＡＭで行われるループ閉じ込み（Ｌｏｏｐ Ｃｌｏｓｕｒｅ）と呼ばれる処理である。

実施の形態１に係る３次元地図作成装置１００は、対象物及びその周辺のみで高精度に位置及び姿勢の推定をすることを可能にする大規模な第２の３次元地図を作成する。具体的には、実施の形態１に係る３次元地図作成装置１００は、点検対象機器などの対象物の位置を含むレイアウトが描かれたフロアマップ３００上に１つ以上の第１の３次元地図（すなわち、小規模な３次元地図）を配置することによって、１つの第２の３次元地図（すなわち、大規模な３次元地図）を作成する。

３次元地図作成装置１００がタブレットＰＣである場合、タブレットＰＣを携帯したユーザが対象物の正面まで移動すると、３次元地図作成装置１００は、フロアマップの１つ以上の領域に１つ以上の第１の３次元地図をそれぞれ登録することによって、図２（Ａ）に示されるように、対象物Ａ１〜Ａ３が表示された画像２１１における適切な位置にコンテンツ２１２を重畳表示する。

図４は、実施の形態１に係る３次元地図作成装置１００の構成を概略的に示す機能ブロック図である。図４に示されるように、３次元地図作成装置１００は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成する３次元地図生成部１１０と、フロアのフロアマップを取得し、３次元地図生成部１１０によって生成された１つ以上の第１の３次元地図をフロアマップ上に配置することによって、１つ以上の第１の３次元地図を含む大規模な第２の３次元地図を生成するフロアマップ登録部１２０と、第２の３次元地図を複数の範囲に分割する範囲分割部１３０とを有する。また、３次元地図作成装置１００は、範囲指定部１４０と、位置姿勢推定部１５０とを有してもよい。

〈３次元地図生成部１１０〉
図５（Ａ）は、フロアマップ３００の例を示す平面図であり、図５（Ｂ）は、小規模な第１の３次元地図４００で表された対象物の例を示す斜視図である。３次元地図生成部１１０は、例えば、ＳＬＡＭなどを用いて１つ以上の第１の３次元地図を生成する。第１の３次元地図４００は、フロア上の対象物及びその周辺の小規模な領域の３次元地図である。フロアマップ３００には、対象物の配置場所である対応領域３０１〜３０５が描かれている。例えば、第１の３次元地図４００は、回転、平行移動、及びスケール調整のいずれか１つ以上が実行され、対応領域３０３に当てはめられる。

〈フロアマップ登録部１２０〉
図６（Ａ）は、第１の３次元地図４００のｘｙｚ軸周りの回転を示す斜視図であり、図６（Ｂ）は、第１の３次元地図４００の地面に平行なフロアマップ３００の法線周りの回転（すなわち、ｙ軸周りの±Ｒｙ方向の回転）を示す平面図である。実施の形態１では、フロアマップ登録部１２０は、図６（Ｂ）に示されるように、対応領域３０１〜３０５が描かれているフロアマップ３００を、例えば、外部の記憶装置から取得する。フロアマップ登録部１２０は、対応領域３０１〜３０５が描かれているフロアマップ３００を予め記憶する記憶部を有してもよい。或いは、フロアマップ登録部１２０は、対応領域３０１〜３０５が描かれているフロアマップ３００を、操作入力部からのユーザ入力操作に従って、外部の記憶装置又はネットワーク上のサーバから取得してもよい。

例えば、フロアマップ３００の中から、小規模な第１の３次元地図として再構成された対応領域３０３が、ユーザ操作によって指定される。この際、図６（Ａ）に示されるように、ｘｙｚ軸周りの３つの回転調整は、ユーザにとって煩雑な作業であるため、フロアマップ登録部１２０は、図６（Ｂ）に示されるように、フロアマップ登録に必要な１つの回転軸周りの調整（例えば、ｙ軸周りの±Ｒｙ方向の回転）を自動的に選択して、不要な回転軸の調整（例えば、ｘ軸周りの±Ｒｘ方向の回転及びｚ軸周りの±Ｒｚ方向の回転）を無効化する処理を行ってもよい。この処理を加えることで、３次元的な回転を調整することなく、ユーザは、フロアマップ３００上の指定の対応領域（例えば、３０３）に第１の３次元地図（例えば、４００）を登録することが可能である。例えば、ユーザ操作によって第１の３次元地図の地面の法線周りの回転角度と平行移動量とを指定することで、フロアマップ登録部１２０は、１つ以上の第１の３次元地図をフロアマップ３００に登録する。

図７は、図４に示されるフロアマップ登録部１２０の構成を概略的に示す機能ブロック図である。フロアマップ登録部１２０は、地面検出部１２１と、外部パラメータ計算部１２２と、外部パラメータ入力部１２３と、外部パラメータ作用部１２４とを有する。

地面検出部１２１は、３次元地図に基づいて地面を検出する。地面検出方法の例として、ロバスト推定のアルゴリズムの１つであるＲａｎｄｏｍ Ｓａｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ（ＲＡＮＳＡＣ）などを用いる方法がある。平面の係数（ａ ｂ ｃ ｄ）^Ｔと、平面上の位置（ｘ ｙ ｚ）^Ｔの関係は、以下の式（１）で表される。

式（１）において、「・」は、内積を示す。ＲＡＮＳＡＣを用いて求められた平面は、無限平面であるため、地面検出部１２１は、凸包（Ｃｏｎｖｅｘ ｈｕｌｌ）などを用いて平面の範囲を計算する。地面検出部１２１は、範囲が最も大きい平面を地面として検出する。

範囲が最も大きい平面が地面とは、限らないケースもあるため、地面検出部１２１は、以下の方法で地面を検出してもよい。例えば、地面検出部１２１は、ユーザ入力操作に基づいて、複数の平面の中から地面（すなわち、水平面）を検出してもよい。或いは、地面検出部１２１は、慣性計測装置（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：ＩＭＵ）によって計測された加速度から重力方向を求め、法線が重力に近い平面を地面と判断してもよい。或いは、地面検出部１２１は、ＩＭＵによって計測された重力方向と、平面の面積の大きさを用いて地面を検出してもよい。

外部パラメータ計算部１２２は、地面検出部１２１によって検出された地面とフロアマップ３００との関係性から外部パラメータＴ_１を計算する。まず、外部パラメータ計算部１２２は、検出した地面の法線ｎ_ｇと、フロアマップ３００の法線ｎ_ｆとから回転Ｒ_１を求める。ｎ_ｇとｎ_ｆは、以下の式（２）及び（３）で表される。

フロアマップ３００は、図８に示されるようなｘｙｚ直交座標系で示されると仮定する。外部パラメータ計算部１２２は、回転Ｒ_１を、式（２）に示されるｎ_ｇのｘ方向に対する偏角θ_ｘと式（３）に示されるｎ_ｆのｚ方向に対する偏角θ_ｚとを用いて計算する。偏角θ_ｘ、θ_ｚを用いて求めた回転行列をそれぞれＲ_ｘ、Ｒ_ｚとすると、これらは、式（４）で示され、回転Ｒ_１は式（５）で示される。

外部パラメータ計算部１２２は、式（５）で回転Ｒ_１を求めた後、平面上の一点のベクトルｘ_ｐから

を計算する。

高さ方向に相当するｙ座標であるｈ_ｙを平行移動量とすると、外部パラメータＴ_１は、回転Ｒ_１及びベクトルｔ_１から、以下の式（６）によって表される。

式（６）において、ベクトルｔ_１は、回転Ｒ_１及びベクトルｘ_ｐの高さ方向のみを取り出したベクトルであり、以下の式（７）のように表される。

外部パラメータ入力部１２３は、ユーザ入力を受け付ける。外部パラメータ入力部１２３におけるユーザ入力により、一部の外部パラメータが入力される。外部パラメータ入力部１２３に入力される外部パラメータは、例えば、図８に示されるフロアマップ３００の座標系においてｙ軸周りの回転Ｒ_２と、ｘｚ平面における平行移動量ｔ_２である。これらから、外部パラメータ入力部１２３は、外部パラメータＴ_２を以下の式（８）によって取得する。また、ユーザ入力を回転の角度θ_ｙとすると、回転Ｒ_２は以下の式（９）によって表される。

なお、図８において、ｔ_２は、フロアマップ３００の左下の点を基準（すなわち、原点）とした平行移動量である。

ｙ軸周りの回転の角度θは、地磁気センサ２５を用いて取得してもよい。角度θの値は、地磁気センサ２５から自動的に入力される。或いは、角度θは、地磁気センサ２５の検出値を初期値としてユーザが入力してもよい。

角度θ及び平行移動量ｔ_２の入力方法は、Ｇｒａｐｈｉｃｓ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ（ＧＵＩ）の操作、キーボードなどからの数値入力のいずれであってもよい。また、平行移動量ｔ_２の原点は、図８に示されるフロアマップの左下の点と異なる点であってもよい。

外部パラメータ作用部１２４は、外部パラメータ計算部１２２で計算された外部パラメータ及び外部パラメータ入力部１２３から入力された外部パラメータを３次元地図に対して作用させる。３次元地図で管理している点群に対して行う外部パラメータ作用は、以下の式（１０）に示される。ここで、ベクトルｐは、作用前の点群の一点、ベクトルｐ′は、作用後の点を示す。

〈範囲分割部１３０〉
フロアマップに、互いに同じ形状で同じ模様の複数の対象物が並んでいる場合、目標の対象物の位置及び姿勢の推定が失敗する可能性がある。例えば、同じ形状で同じ模様の複数の対象物Ｂ１〜Ｂ６が並んでいる環境では、対象物Ｂ１をスキャンして位置及び姿勢を推定する場合、間違って対象物Ｂ２〜Ｂ６のいずれかの位置及び姿勢が出力される可能性がある。誤推定を避けるために、位置及び姿勢の推定時には、まず、ユーザがいる範囲を指定することが望ましい。範囲を指定することで、範囲内に含まれている対象物の３次元地図４００を元に、詳細な位置及び姿勢を推定することが可能になる。

図９は、図４に示される範囲分割部１３０によって実行される範囲分割処理を示す図である。範囲分割部１３０は、例えば、フロアマップ３００で表される１つのフロアを複数の範囲（例えば、範囲＃１〜＃４）に分割する。範囲分割部１３０は、例えば、ビルの１つのフロアを複数の範囲＃１〜＃４に静的に分割し、複数の範囲＃１〜＃４の各々における３次元地図を管理する。範囲分割部１３０は、例えば、図９に示されるように、１つのフロアマップ３００を４つの範囲＃１〜＃４に分割する。このように、１つのフロアマップ３００を複数の範囲に分割し、同じ模様及び同じ形状を持つ複数の対象物が互いに異なる範囲に属するようにして３次元地図を管理すれば、目標の対象物を、これと同じ形状で同じ模様の他の対象物であると誤って誤推定する可能性は低下する。しかし、この場合には、誤推定が発生する可能性を十分に低減できない。

このような理由から、範囲分割部１３０は、動的な範囲分割を行うことが望ましい。図１０は、図４に示される範囲分割部１３０の構成を概略的に示す機能ブロック図である。範囲分割部１３０は、画像特徴算出部１３１と、クラスタリング部１３２とを有する。

画像特徴算出部１３１は、３次元地図を生成するときにカメラ撮影した画像の類似度に基づいて画像の特徴を算出する。画像特徴算出部１３１は、例えば、Ｂａｇ ｏｆ Ｗｏｒｄｓ（ＢｏＷ）を用いて範囲分割のための処理を行う。画像特徴算出部１３１は、対象物の第１の３次元地図の各々のカメラ撮影画像に対してＢｏＷを使って画像をベクトル化する。画像特徴算出部１３１は、ベクトル同士の距離が近ければ「類似している」と判断する。

クラスタリング部１３２は、画像特徴算出部１３１の結果を元にクラスタリングを行い、範囲分割する。つまり、クラスタリング部１３２は、これらのベクトルが互いに異なる範囲に属するように、範囲分割処理を行う。

〈範囲指定部１４０〉
図１１は、実施の形態１に係る３次元地図作成装置１００の範囲指定部１４０を用いた範囲指定操作時の画面を示す図である。範囲指定部１４０は、範囲分割部１３０によって決定され複数の範囲の中から、１つの範囲を指定する。範囲指定部１４０は、例えば、ユーザ操作部からのユーザ操作によって選択された１つの範囲を指定する。ユーザ操作部は、範囲指定部１４０に接続された装置又は範囲指定部１４０の一部である。ユーザ操作部は、例えば、図１１に示されるようなタブレットＰＣのタッチパネルである。ユーザが、タッチパネルに表示された複数の範囲の中から、タップ操作などよって１つの範囲を選択すると、範囲指定部１４０は、選択された範囲を指定する。

〈位置姿勢推定部１５０〉
位置姿勢推定部１５０は、範囲指定部１４０によって指定された範囲に含まれる３次元地図を用いて、対象物の位置及び姿勢を推定する。位置及び姿勢を推定する技術の例として、ＢｏＷとＰｅｒｓｐｅｃｔｉｖｅ Ｎ Ｐｏｉｎｔｓ（ＰｎＰ）とを組み合わせる方法がある。

《１−２》動作
図１２（Ａ）は、３次元地図生成処理を示すフローチャートであり、図１２（Ｂ）は、位置姿勢推定処理を示すフローチャートである。まず、図１２（Ａ）に示されるように、３次元地図生成部１１０は、対象物毎に小規模な第１の３次元地図を生成する（ステップＳ１１、Ｓ１２）。次に、フロアマップ登録部１２０は、第１の３次元地図をフロアマップ３００に登録する（ステップＳ１３）。ステップＳ１１〜Ｓ１３の処理は、対象物の個数分、繰り返され、その結果、大規模の第２の３次元地図が生成される。全ての対象物の登録が終了した後、範囲分割部１３０は、第２の３次元地図を複数の範囲に分割する（ステップＳ１４）。

次に、図１２（Ｂ）に示されるように、３次元地図を用いた位置及び姿勢の推定が行われる。まず、範囲指定部１４０は、ユーザ操作に基づいて、ユーザがいるおおよその範囲である指定範囲を選択する（ステップＳ２１）。その後、位置姿勢推定部１５０は、指定範囲内の３次元地図を使って位置及び姿勢の推定を行う（ステップＳ２２）。

図１３は、３次元地図作成装置１００のフロアマップ登録部１２０によって実行されるフロアマップ登録処理（ステップＳ１３）を示すフローチャートである。まず、地面検出部１２１は、地面の検出を行う（ステップＳ１３１〜Ｓ１３５）。例えば、地面検出部１２１は、複数の平面を検出した後（ステップＳ１３１〜Ｓ１３３）、その中から地面とみなすことができる最大の平面を求めることで地面を検出する（ステップＳ１３４、Ｓ１３５）。その後、外部パラメータ計算部１２２は、地面とフロアマップ３００の関係を使った移動量を算出し、外部パラメータ入力部１２３はユーザ操作で入力された平行移動量と地面法線周りの回転量とから移動量を求める（ステップＳ１３６、Ｓ１３７）。次に、外部パラメータ作用部１２４は、これら２つの移動量である外部パラメータを３次元地図に作用させる（ステップＳ１３８）。

図１４は、３次元地図作成装置１００の範囲分割部１３０によって実行される範囲分割処理を示すフローチャートである。まず、画像特徴算出部１３１は、ＢｏＷなどを用いて画像特徴を算出し、画像特徴が互いに類似する場合には、この情報をルールに追加する。３次元地図の最初のループ（ステップＳ１４１）のインデックスをｉ（ここで、ｉは０以上Ｎ以下の整数）、２番目のループ（ステップＳ１４３）のインデックスをｊ（ここで、ｊは０以上Ｎ以下の整数）とすると、画像特徴算出部１３１は、ｉ＜ｊのときに３次元地図の類似度を計算する（ステップＳ１４１〜Ｓ１４７）。図１５は、類似度を計算する場合と計算しない場合とを説明するための図である。つまり、図１５の白色のときに類似度を計算し、斜線範囲のときに類似度を計算しない。

図１６は、３次元地図作成装置１００の範囲分割部１３０によって実行される類似判定処理を示すフローチャートである。図１６に示されるように、範囲分割部１３０の画像特徴算出部１３１は、まず、２つの画像特徴の距離を計算する。ここでの画像特徴を、ベクトルｖ_１、ベクトルｖ_２とすると、範囲分割部１３０は、例えば、以下の式（１１）で表されるユークリッド距離を、２つの画像特徴の距離として用いる。

画像特徴算出部１３１は、２つの画像特徴の距離の計算を計算し（ステップＳ１４０３）、最小距離を求める処理（ステップＳ１４０４、Ｓ１４０５）を３次元地図の数であるＮ（ｊ）回繰り返し（ステップＳＴ１４０１）、さらに、ステップＳ１４０１〜Ｓ１４０４の処理を３次元地図の数であるＮ（ｉ）回繰り返す（ステップＳＴ１４００）。画像特徴算出部１３１は、画像特徴の最小距離を求めた後、最小距離がα未満の場合（ステップＳ１４０５においてｔｒｕｅ）、類似と判定して処理を終了し、最小距離がα以上の場合（ステップＳ１４０５においてｆａｌｓｅ）、非類似と判定して処理を終了する。ここでのαは、予め設定された値であり、例えば、開発者又はユーザなどが設定するパラメータである。

上記の処理でルールが決定した後、クラスタリング部１３２は、ルールに基づいてクラスタリングを実行する（図１４におけるステップＳ１４８〜Ｓ１５１）。図１４は、階層型クラスタリングである分割型クラスタリング（Ｄｉｖｉｓｉｖｅ Ｃｌｕｓｔｅｒｉｎｇ）を用いてクラスタリングを行う例を示す。まず、クラスタリング部１３２は、分割対象の３次元地図を範囲分割する。ここで分割する３次元地図をＣ１，Ｃ２とする。クラスタリング部１３２は、３次元地図Ｃ１の中心位置に近い他の３次元地図は、Ｃ１のクラスに属し、３次元地図Ｃ２の中心位置に近い他の３次元地図は、Ｃ２のクラスに属するように、範囲分割を行う。クラスタリング部１３２は、このような３次元地図の範囲分割を繰り返して、最終的な範囲分割を行う。

《１−３》効果
以上に説明したように、実施の形態１に係る３次元地図作成装置１００は、小規模な１つ以上の第１の３次元地図をＳＬＡＭを用いて作成し、フロアマップ３００上に１つ以上の第１の３次元地図を配置することによって大規模な第２の３次元地図（例えば、屋内環境全体の３次元地図）を作成している。第１の３次元地図を作成するときに、ＳＬＡＭによって蓄積する位置誤差は小さいので、第２の３次元地図における位置誤差は抑制される。したがって、実施の形態１に係る３次元地図作成装置１００は、位置誤差の小さい大規模な３次元地図を作製することができる。

《２》実施の形態２
《２−１》構成
図１７は、実施の形態２に係る３次元地図作成装置１０１の構成を概略的に示す機能ブロック図である。図１７において、図４に示される構成要素と同一又は対応する構成要素には、図４に示される符号と同じ符号が付されている。実施の形態２に係る３次元地図作成装置１０１は、コンテンツ登録部１６０及びコンテンツ重畳表示部１７０を有する点において、実施の形態１に係る３次元地図作成装置１００と異なる。なお、３次元地図作成装置１０１のＨＷ構成は、図３に示されるものと同じである。

コンテンツ登録部１６０は、１つ以上の対象物の各々の第１の３次元地図を用いて、各対象物を表示する画像に重ねて表示するコンテンツの位置を登録する。コンテンツ重畳表示部１７０は、カメラ２２で撮影した画像に、登録されているコンテンツを重畳してディスプレイ３０に表示させる。コンテンツは、例えば、線若しくは平面などの図形と対象物を示す情報、立方体若しくは球などの３次元の図形と対象物を示す情報、又はこれらの組み合わせなどを含むことができる。

《２−２》動作
図１８（Ａ）は、３次元地図作成装置１０１が行う３次元地図生成処理を示すフローチャートであり、図１８（Ｂ）は、３次元地図作成装置１０１がコンテンツ重畳表示のために行う処理を示すフローチャートである。図１８（Ａ）及び（Ｂ）において、図１２（Ａ）及び（Ｂ）と同じ処理ステップには、図１２（Ａ）及び（Ｂ）の処理ステップにおける符号と同じ符号が付されている。図１８（Ａ）に示されるように、実施の形態２に係る３次元地図作成装置１０１は、各対象物を表示する画像に重ねて表示するコンテンツの位置を登録するコンテンツ登録処理（ステップＳ１５）を行う点において、実施の形態１に係る３次元地図作成装置１００と異なる。また、図１８（Ｂ）に示されるように、実施の形態２に係る３次元地図作成装置１０１は、コンテンツ登録処理で登録されたコンテンツを各対象物を表示する画像に重ねて表示するコンテンツ表示（ステップＳ２３）を行う点において、実施の形態１に係る３次元地図作成装置１００と異なる。

《２−３》効果
以上に説明したように、実施の形態２に係る３次元地図作成装置１０１は、小規模な１つ以上の第１の３次元地図をＳＬＡＭを用いて作成し、フロアマップ３００上に１つ以上の第１の３次元地図を配置することによって大規模な第２の３次元地図（例えば、屋内環境全体の３次元地図）を作成している。第１の３次元地図を作成するときに、ＳＬＡＭによって蓄積する位置誤差は小さいので、第２の３次元地図における位置誤差は抑制される。したがって、実施の形態２に係る３次元地図作成装置１０１は、図２（Ａ）に示されるように、コンテンツを適切な位置に重畳表示することができる。

上記以外の点に関し、実施の形態２は、実施の形態１と同じである。

《３》実施の形態３
《３−１》構成
図１９は、実施の形態３に係る３次元地図作成装置１０２の構成を概略的に示す機能ブロック図である。図１９において、図４に示される構成要素と同一又は対応する構成要素には、図４に示される符号と同じ符号が付されている。実施の形態３に係る３次元地図作成装置１０２は、相対移動距離推定部１８０を有する点において、実施の形態１に係る３次元地図作成装置１００と異なる。なお、３次元地図作成装置１０２のＨＷ構成は、図３に示されるものと同じである。

実施の形態１において説明したように、位置姿勢推定部１５０は、対象物及びその周辺における小規模な第１の３次元地図に基づいて、対象物の位置及び姿勢を推定する。しかし、第１の３次元地図から離れた場所に存在する物体の位置及び姿勢を推定するためには、位置及び姿勢の推定が成功した場所からの相対的な移動距離を求める必要がある。実施の形態３に係る３次元地図作成装置１０２は、相対移動距離推定部１８０を設けることによって、相対的な移動距離を算出する機能を有している。

《３−２》動作
図２０（Ａ）は、３次元地図作成装置１０２が行う３次元地図生成処理を示すフローチャートであり、図２０（Ｂ）は、３次元地図作成装置１０２が行う位置姿勢推定処理を示すフローチャートである。図２０（Ａ）及び（Ｂ）において、図１２（Ａ）及び（Ｂ）と同じ処理ステップには、図１２（Ａ）及び（Ｂ）の処理ステップにおける符号と同じ符号が付されている。図２０（Ｂ）に示されるように、実施の形態３に係る３次元地図作成装置１０２は、相対的な移動距離を求める処理（ステップＳ２４、Ｓ２５）を行う点において、実施の形態１に係る３次元地図作成装置１００と異なる。

相対的な移動距離の推定には、例えば、以下の方法が使用可能である。第１の方法は、ＳＬＡＭを用いる方法である。これは、カメラで撮影した画像、又は距離センサ２１で検出されたセンサデータ、又はこれらの両方を使って移動量を求める方法である。これは、例えば、時間方向に並ぶ２つのフレーム間の移動量を、順次積算することによって移動量を求める方法である。

第２の方法は、カメラで撮影した画像及び距離センサ２１で検出されたセンサデータに、ジャイロセンサ２３、加速度センサ２４、地磁気センサ２５のいずれか１つ以上を使ったデッドレコニング（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ）を組み合わせた方法である。Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇは、ジャイロセンサ２３、加速度センサ２４、地磁気センサ２５などを使って移動量を求める方法である。この方法は、加速度の積分とジャイロセンサ２３により移動速度と移動方向を求め、速度を積分することで移動距離を求める方法である。

第３の方法は、歩行者デッドレコニング（Ｐｅｄｅｓｔｒｉａｎ Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ）を用いる方法である。第３の方法は、ジャイロセンサ２３及び加速度センサ２４のセンサデータから歩幅及び歩数を求め、これらから移動距離を求める方法である。

《３−３》効果
以上に説明したように、実施の形態３に係る３次元地図作成装置１０２は、小規模な１つ以上の第１の３次元地図をＳＬＡＭを用いて作成し、フロアマップ３００上に１つ以上の第１の３次元地図を配置することによって大規模な１つの第２の３次元地図（例えば、屋内環境全体の３次元地図）を作成している。第１の３次元地図を作成するときにＳＬＡＭによって蓄積する位置誤差は小さいので、第２の３次元地図における位置誤差は低減される。したがって、実施の形態３に係る３次元地図作成装置１０２は、図２（Ａ）に示されるように、コンテンツを適切な位置に重畳表示することができる。さらに、第１の３次元地図から離れた場所でも位置及び姿勢の推定ができる。

上記以外の点に関し、実施の形態３は、実施の形態１と同じである。また、相対移動距離推定部１８０を実施の形態２の構成に備えることも可能である。

１０ コンピュータ、 １１ プロセッサ、 １２ メモリ、 ２１ 距離センサ、 ２２ カメラ、 ２３ ジャイロセンサ、 ２４ 加速度センサ、 ２５ 地磁気センサ、 ３０ ディスプレイ、 ４０ ３次元地図ＤＢ、 １００、１０１、１０２ ３次元地図作成装置、 １１０ ３次元地図生成部、 １２０ フロアマップ登録部、 １２１ 地面検出部、 １２２ 外部パラメータ計算部、 １２３ 外部パラメータ入力部、 １２４ 外部パラメータ作用部、 １３０ 範囲分割部、 １４０ 範囲指定部、 １５０ 位置姿勢推定部、 １６０ コンテンツ登録部、 １７０ コンテンツ重畳表示部、 １８０ 相対移動距離推定部、 ３００ フロアマップ。

本発明の一態様に係る３次元地図作成装置は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に、前記フロアに垂直な方向の高さを持つ前記対象物を含む３次元地図である第１の３次元地図を生成する３次元地図生成部と、前記対象物の位置を含むレイアウトが描かれた前記フロアのフロアマップを取得し、前記３次元地図生成部によって生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記フロアマップと前記１つ以上の第１の３次元地図とを含む第２の３次元地図を生成するフロアマップ登録部とを有する。

本発明の他の態様に係る３次元地図作成方法は、フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に、前記フロアに垂直な方向の高さを持つ前記対象物を含む３次元地図である第１の３次元地図を生成するステップと、前記対象物の位置を含むレイアウトが描かれた前記フロアのフロアマップを取得し、生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記フロアマップと前記１つ以上の第１の３次元地図とを含む第２の３次元地図を生成するステップとを有する。

Claims (9)

1. フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成する３次元地図生成部と、
   前記フロアのフロアマップを取得し、前記３次元地図生成部によって生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記１つ以上の第１の３次元地図を含む第２の３次元地図を生成するフロアマップ登録部と、
   を有することを特徴とする３次元地図作成装置。
2. 前記第２の３次元地図を複数の範囲に分割する範囲分割部をさらに有することを特徴とする請求項１に記載の３次元地図作成装置。
3. 前記複数の範囲のうちのいずれかを指定範囲として指定する範囲指定部と、
   前記１つ以上の第１の３次元地図のうちの前記指定範囲内に配置された第１の３次元地図を用いて、前記指定範囲内に存在する対象物の位置及び姿勢を推定する位置姿勢推定部と、
   をさらに有することを特徴とする請求項２に記載の３次元地図作成装置。
4. 前記フロアマップ登録部は、
   複数の平面を検出し、前記複数の平面のうちから前記フロアに対応する地面を検出する地面検出部と、
   前記地面を用いて前記センサの移動量に基づく第１の外部パラメータを算出する外部パラメータ計算部と、
   ユーザ入力によって取得された移動量に基づく第２の外部パラメータを生成する外部パラメータ入力部と、
   前記第１の外部パラメータ及び前記第２の外部パラメータに基づいて、前記第２の３次元地図を補正する外部パラメータ作用部と
   を有することを特徴とする請求項１から３のいずれか１項に記載の３次元地図作成装置。
5. 前記範囲分割部は、
   前記第１の３次元地図の特徴の類似度に基づいて画像の特徴を算出する画像特徴算出部と、
   前記類似度に基づいて、前記範囲の分割を行うクラスタリング部と、
   を有することを特徴とする請求項２又は３に記載の３次元地図作成装置。
6. 前記対象物を含む領域毎の第１の３次元地図を用いて、各対象物を表示する画像に重ねて表示するコンテンツの位置を登録するコンテンツ登録部と、
   カメラで撮影した画像に前記コンテンツを重畳してディスプレイに表示させコンテンツ重畳表示部と、
   をさらに有することを特徴とする請求項１から５のいずれか１項に記載の３次元地図作成装置。
7. 前記第１の３次元地図から離れた場所に存在する物体までの相対的な移動距離を算出する相対移動距離推定部をさらに有することを特徴とする請求項１から６のいずれか１項に記載の３次元地図作成装置。
8. フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成するステップと、
   前記フロアのフロアマップを取得し、生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記１つ以上の第１の３次元地図を含む第２の３次元地図を生成するステップと、
   を有することを特徴とする３次元地図作成方法。
9. コンピュータに、
   フロア上を移動するセンサによって検出されたセンサデータに基づいて、対象物を含む領域毎に第１の３次元地図を生成する処理と、
   前記フロアのフロアマップを取得し、生成された１つ以上の第１の３次元地図を前記フロアマップ上に配置することによって、前記１つ以上の第１の３次元地図を含む第２の３次元地図を生成する処理と、
   を実行させることを特徴とする３次元地図作成プログラム。

Images