知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-04-08
(45)【発行日】2024-04-16
(54)【発明の名称】位置姿勢算出方法および位置姿勢算出プログラム
(51)【国際特許分類】
G01C 21/30 20060101AFI20240409BHJP
G06T 7/70 20170101ALI20240409BHJP
G08G 1/16 20060101ALI20240409BHJP
【ＦＩ】
G01C21/30
G06T7/70 Z
G08G1/16 A
【請求項の数】
15
(21)【出願番号】P 2020144088
(22)【出願日】2020-08-28
(65)【公開番号】P2022039188
(43)【公開日】2022-03-10
【審査請求日】2023-05-11
(73)【特許権者】
【識別番号】000005223
【氏名又は名称】富士通株式会社
(74)【代理人】
【識別番号】100104190
【弁理士】
【氏名又は名称】酒井 昭徳
(72)【発明者】
【氏名】北浦 麻子
(72)【発明者】
【氏名】藤田 卓志
【審査官】宮本 礼子
(56)【参考文献】
【文献】特開２０１７－１４２１４７（ＪＰ，Ａ）
【文献】特開２０１２－１７３８４３（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０８Ｇ １／００－９９／００
Ｇ０１Ｃ ２１／００－２５／００
Ｇ０６Ｔ ７／７０
(57)【特許請求の範囲】
【請求項1】
情報処理装置が、移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定し、
前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動方向が類似する経路情報を有する環境マップである、
ことを特徴とする位置姿勢算出方法。
【請求項2】
情報処理装置が、移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定し、
前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動経路および移動方向が類似する経路情報を有する環境マップである、
ことを特徴とする位置姿勢算出方法。
【請求項3】
前記移動方向は、前記取得装置の種類、前記取得装置の設置位置、前記取得装置が取得するデータの取得方向の少なくともいずれか一つに基づいて補正する、ことを特徴とする請求項１または２に記載の位置姿勢算出方法。
【請求項4】
前記移動経路は、前記取得装置の種類、前記取得装置の設置位置、前記取得装置が取得するデータの取得方向の少なくともいずれか一つに基づいて補正する、ことを特徴とする請求項２に記載の位置姿勢算出方法。
【請求項5】
情報処理装置が、
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定し、
前記算出用環境マップの特定は、前記環境マップの経路情報の各位置、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得したデータの各取得位置を中心とした任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいておこなう、
ことを特徴とする位置姿勢算出方法。
【請求項6】
情報処理装置が、
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定し、
前記算出用環境マップの特定は、前記環境マップの経路情報、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報における経路を含む任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいておこなう、
ことを特徴とする位置姿勢算出方法。
【請求項7】
前記情報処理装置が、前記経路の重なり度に基づいて、特定された前記算出用環境マップの利用優先度を算出する、
ことを特徴とする請求項５または６に記載の位置姿勢算出方法。
【請求項8】
情報処理装置が、
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定し、
前記情報処理装置が、
前記複数の環境マップを、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出をおこなう他の情報処理装置にあらかじめメモリ展開しておき、
特定された前記算出用環境マップがメモリ展開されている情報処理装置を特定する、
ことを特徴とする位置姿勢算出方法。
【請求項9】
前記情報処理装置が、前記環境マップに、当該環境マップがメモリ展開されている情報処理装置に関する情報を登録する、
ことを特徴とする請求項８に記載の位置姿勢算出方法。
【請求項10】
前記情報処理装置が、特定された情報処理装置によって算出された位置姿勢結果を取得する、
ことを特徴とする請求項８または９に記載の位置姿勢算出方法。
【請求項11】
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させ、
前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動方向が類似する経路情報を有する環境マップである、
ことを特徴とする位置姿勢算出プログラム。
【請求項12】
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させ、
前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動経路および移動方向が類似する経路情報を有する環境マップである、
ことを特徴とする位置姿勢算出プログラム。
【請求項13】
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させ、
前記算出用環境マップを特定する処理は、前記環境マップの経路情報の各位置、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得したデータの各取得位置を中心とした任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいておこなう、
ことを特徴とする位置姿勢算出プログラム。
【請求項14】
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させ、
前記算出用環境マップを特定する処理は、前記環境マップの経路情報、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報における経路を含む任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいておこなう、
ことを特徴とする位置姿勢算出プログラム。
【請求項15】
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させ、
前記情報処理装置に、
前記複数の環境マップを、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出をおこなう他の情報処理装置にあらかじめメモリ展開しておき、
特定された前記算出用環境マップがメモリ展開されている情報処理装置を特定する処理を実行させる、
ことを特徴とする位置姿勢算出プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体が取得するデータの取得位置と姿勢を算出する位置姿勢算出方法および位置姿勢算出プログラムに関する。
【背景技術】
【0002】
従来、移動体が移動中に取得した周辺状況に関するデータを入力とし、移動体の走行経路と周辺環境地図を同時に作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）という技術がある。また、ＳＬＡＭ技術の中でも、移動体が撮影した映像などの動画像を入力として、移動体の走行時のカメラ位置・姿勢を推定する技術は、Ｖｉｓｕａｌ－ＳＬＡＭ（以後、「Ｖ－ＳＬＡＭ」という）と呼ばれる。
【0003】
Ｖ－ＳＬＡＭは、移動体が取得するデータ（以下「車載データ」という）の一例である車載カメラで撮影した動画像を入力とし、撮影した動画像に映る被写体の変化を用いて、自車走行経路（自車位置・姿勢）と周辺環境地図（周辺の被写体の画像特徴点群の３次元位置マップ、以下、「３Ｄ環境マップ」あるいは、単に「マップ」という）を推定作成することができる技術であり、動画像から移動体の自車位置と姿勢を算出し、推定することができる。
【0004】
また、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）は、光を用いたリモートセンシング技術の一つであり、パルス状に発光するレーザー照射に対する散乱光を測定し、遠距離にある対象までの距離やその対象の性質を分析することができる。ＳＬＡＭでは、車載データの一例であるＬｉＤＡＲデータのような任意の電磁波やレーザー光を用いた測距センサデータを使って、移動体の自車位置と姿勢を算出し、推定することもできる。
【0005】
近年、コネクテッドカーの普及などに伴い、車両などの移動体の車載機・ドライブレコーダーのデータ（映像など）をセンターに集めて分析することで、車両を周辺地物状況を把握するセンサとして活用するサービスが検討されている。たとえば、落下物の場所を特定して利用するイベント発生検知サービスや、地図更新のため、地物の設置位置変化を把握する地図変化検知サービスなどがある。
【0006】
関連する先行技術としては、Ｖ－ＳＬＡＭを用いた高精度な位置・姿勢推定をおこなう技術がある。
【先行技術文献】
【特許文献】
【0007】
【文献】特開２０２０－６７４３９号公報
【発明の概要】
【発明が解決しようとする課題】
【0008】
しかしながら、周辺変化に迅速に対応してこれらのサービスを実現するには、動画像やＬｉＤＡＲデータなどの車載データ取得時の自車位置・姿勢、すなわち、車載画像を撮影したカメラ位置・姿勢あるいは車載データの取得位置・姿勢を実座標系で、できるだけ高速にかつ高精度に推定する必要がある。
【0009】
一つの側面では、本発明は、移動体または当該移動体に搭載される取得装置が取得するデータの取得位置と姿勢を高速にかつ高精度に求めることを目的とする。
【課題を解決するための手段】
【0010】
一つの実施態様では、情報処理装置が、移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する位置姿勢算出方法が提供される。
【発明の効果】
【0011】
本発明の一側面によれば、移動体または当該移動体に搭載される取得装置が取得するデータの取得位置と姿勢を高速にかつ高精度に求めることができる。
【図面の簡単な説明】
【0012】
【発明を実施するための形態】
【0013】
以下に図面を参照して、本発明にかかる位置姿勢算出方法および位置姿勢算出プログラムの実施の形態を詳細に説明する。
【0014】
（位置姿勢算出方法および位置姿勢算出プログラムの概要）
周辺環境地図である３Ｄ環境マップは、車載データのデータ特徴の３Ｄ位置群（３Ｄデータ特徴点群）からなり、任意の車載データに対し、マップのどの３Ｄマップ要素（３Ｄ画像特徴点）が車載データのどこに写りこんでいるか、を解析して、どこから周辺状況のデータを取得したか、すなわち、車載データのセンサ（たとえば撮影装置や測距装置）の位置姿勢を推定（算出）する。
周辺環境地図である３Ｄ環境マップは、車載データのデータ特徴の３Ｄ位置群（３Ｄデータ特徴点群）からなり、任意の車載データに対し、マップのどの３Ｄマップ要素（３Ｄ画像特徴点）が車載データのどこに写りこんでいるか、を解析して、どこから周辺状況のデータを取得したか、すなわち、車載データのセンサ（たとえば撮影装置や測距装置）の位置姿勢を推定（算出）する。
【0015】
３Ｄ環境マップを用いた、車載データの一例である動画像の撮影位置姿勢を推定するには、一般的にＶ－ＳＬＡＭを用いることができる。Ｖ－ＳＬＡＭは、動画像を入力して、各画像の画像特徴点群を抽出し、各画像特徴点群の画像内の見え変化から画像特徴３Ｄ位置（３Ｄ画像特徴群のマップ）と同時に、各画像の撮影位置姿勢を推定する。さらに、動画像取得時のＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）情報（たとえばＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ））を入力として用いることで、実座標系での撮影位置姿勢を高精度に推定することができる。
【0016】
実座標系とは、世界座標系とも呼ばれ、世界における場所および方向を一意に表現可能な座標系であり、緯度・経度、標高、方位などが定義される。実座標系の定義方法は多種あるが、相互変換可能である。
【0017】
また、撮影位置姿勢を推定するたびに毎回、３Ｄ環境マップを作成するのではなく、作成した３Ｄ環境マップを保存しておく。そして、同じ道路を走行した別の動画像の撮影位置姿勢を推定する際に、その作成保存済の３Ｄ環境マップを使うことで、撮影位置姿勢の推定処理を省力化し、利用する３Ｄ環境マップと同じ座標系（同じ精度の実座標系）で撮影位置姿勢を算出することができる。
【0018】
このように、３Ｄ環境マップを整備しておけば、より高速かつ少ない計算量で位置姿勢推定（算出）を実行することができるため、広範囲にサービスを実施する際は、サービス範囲内の各道路の３Ｄ環境マップを蓄積管理することが望ましい。さらに、シーンによる映像変化に耐性のある３Ｄ環境マップを配備するためには、地物の見えが変わる道路の上り下りや、天候や昼夜などの時刻、街路樹変化の大きな季節などによって、同じ道路でも複数の３Ｄ環境マップを作成管理する場合が考えられ、より多くの３Ｄ環境マップを蓄積管理することが望ましい。
【0019】
このため、作成管理した３Ｄ環境マップを使ってリアルタイムかつ低コストで撮影位置姿勢を推定するには、より迅速に大量の３Ｄ環境マップ群から利用するマップを特定する方法が必要である。
【0020】
一般的に、従来の広範囲の地図群の蓄積データベースから、任意の走行に合致する地図を検索する方法として、地域メッシュを用いる方法がある。すなわち、配備地図群を地域メッシュごとに分け、地図を要する任意走行時の位置情報（緯度経度情報、撮影位置姿勢推定の場合は、推定対象の動画像に付随するＧＰＳ情報）から現在の地域メッシュを特定して、該地域メッシュに対応する地図群を取得する方法である。
【0021】
地域メッシュとは、日本全国を緯度経度で分割した地図上の方眼区域のことである。国土情報のデジタル化、統計情報の管理区域として使うため、総務省が規定している（ＪＩＳＸ０４０）。緯度経度値から１つの地域メッシュ番号を計算特定することができる。そして、特定したメッシュ番号の３Ｄ環境マップのデータと、車載データおよびＧＮＳＳデータを用いて、車載データの取得センサおよび移動体の位置姿勢を従来の方法、ＳＬＡＭなどを用いて推定する。
【0022】
この地図検索取得方法は、地域メッシュに属する道路が膨大になり、それらの道路のどれが車載データに関係する道路なのかを特定することができない。したがって、不要なマップを含んだ多数の３Ｄ環境マップを取得することになってしまい、取得時の時間およびメモリコストがかかることから、リアルタイムな位置姿勢推定ができない。
【0023】
また、Ｖ－ＳＬＡＭは、間違った３Ｄ環境マップの場合は位置姿勢推定に失敗し、正しい３Ｄ環境マップの場合だけ位置姿勢推定できるため、間違った３Ｄ環境マップかどうかは位置姿勢推定を実施してみないとわからないので、位置姿勢推定に成功するまで取得したマップを変えて位置姿勢推定処理を繰り返す必要がある。したがって、不要なマップの実行コストがかかる。特に、各マップを順に実行すると、本当に実施すべきマップの実行が遅れ、位置姿勢推定ができるまでに時間がかかって、リアルタイムの位置姿勢推定ができない。
【0024】
また別の一般的な、従来の広範囲の地図群の蓄積データベースから任意の走行に合致する地図を検索する方法として、道路ネットワーク情報などの全国道路の道路位置・接続情報（接続形状を含めた緯度経度情報）を用いる方法がある。すなわち、道路ネットワーク情報などの全国道路の道路位置・接続情報をあらかじめ用意しておき、配備地図群をそれぞれ道路ネットワーク情報の道路リンクのどれに関係しているのかを事前に対応づける。そして、地図を要する任意走行の位置情報（同、緯度経度情報）に関係する道路リンクを特定し、同じ道路リンクに対応づけた配備地図を取得する、という方法である。
【0025】
道路ネットワークとは、全国の道路を交差点などの特徴（緯度経度値を持つノード）をつなぐ接続経路（リンク（道路リンク））として表したデータ情報である。道路ネットワークには、規制速度や、道路幅員、道路車線数、リンクの旅行速度（所要時間）など様々な情報を属性として保持する場合もある。
【0026】
この地図検索手法は、配備する３Ｄ環境マップと、位置姿勢推定したい走行時データ（ＧＰＳなど）の双方に対し、あらかじめ準備した全道路の道路位置・接続情報（道路リンク情報など）にそれぞれ対応づけして検索をおこなう。そのため、配備する３Ｄ環境マップ群とは別に、全道路の道路位置・接続情報（道路リンク情報）を別途配備しないと、位置姿勢推定を実施することができない。特に、この全道路の道路位置・接続情報（道路リンク情報）は、任意地点でＧＰＳと比較できる程度の位置精度が必要である。そのため、道路形状に沿った情報である必要があり、かつ、常に道路変化に合わせて情報内容を更新管理する必要がある。また、データ量も膨大になり、配備後のメンテナンスコストが高い。さらに、道路ネットワーク情報ＤＢの内容が更新されるたびに、３Ｄ環境マップＤＢの対応付けも見直す必要があるため、３Ｄ環境マップＤＢのメンテナンスコストも高くなる。
【0027】
以上のように、従来の方法では、作成した３Ｄ環境マップ群を保持しておいても、位置姿勢推定したい車載データに対応する３Ｄ環境マップを正しく特定することができないため、位置姿勢推定がリアルタイムに実施できなかったり、３Ｄ環境マップを特定するための膨大なデータ管理コストが必要である。本実施の形態にかかる位置姿勢算出方法および位置姿勢算出プログラムは、道路ネットワーク情報などの道路地図情報を用いずに、撮影位置姿勢推定したい車載データに関する３Ｄ環境マップを特定することができる。
【0028】
図１は、各３Ｄ環境マップに対応する道路のイメージを示す説明図である。図１においては、３本の道路１０１、１０２、１０３が示されている。道路１０１は上りと下りの二車線からなる道路であり、道路１０２は、道路１０１と交差する、上りと下りの二車線からなる道路であり、道路１０３は、道路１０１と交差する、上りの一車線（一方通行）からなる道路である。
【0029】
また、図２および図３は、各３Ｄ環境マップ群に属する３Ｄ画像特徴群を模式的に示す説明図である。図２は、上空俯瞰図を示しており、図２においては、図１に示した道路と同じ道路における３Ｄ環境マップ群を示している。したがって、上りと下りの二車線からなる道路１０２が、上りと下りの二車線からなる道路１０１と交差しており、また、上りの一車線（一方通行）からなる道路１０３が、道路１０２と同様に、道路１０１と交差しているのがわかる。
【0030】
そして、図２では、各道路の３Ｄ環境マップは、作成に用いた各車載映像の取得時の走行区間に対応して、複数の異なる道路区間の３Ｄ環境マップとして分割作成されている。よって、３道路１０１～１０３の各上り、下りの合計５つに対して、合計１２個の３Ｄ環境マップが存在する。各道路での各３Ｄ環境マップの範囲、すなわち、どの走行区間の車載映像を用いて３Ｄ環境マップを作成するかは自由であり、たとえば、他の道路と交差する地点（交差点）までの間の区間の３Ｄ環境マップ群として分割作成することができる。あるいは、交差点を跨いだ区間として作成することもできる。
【0031】
図２では、後述する３Ｄ環境マップ５２２の２つの要素である「３Ｄマップ要素」８０１と「データ構造化要素」８０２を示す。「３Ｄマップ要素」８０１は、該当する３次元位置に多数の『〇』（丸形のドット）２０１で示している。図２からもわかるように、多数の『○』は、３Ｄ環境マップ５２２の３Ｄ画像特徴点『○』群を示しており、実座標３Ｄ位置を有する、道路を走行時に画像に写る画像特徴点群である。
【0032】
また、「データ構造化要素」８０２は、一例として「データ構造化要素」８０２を、ある瞬間の車載データで参照できる「３Ｄマップ要素」８０１群を纏めるものとした場合であり、図２では便宜的に、瞬間の車載データを取得した車載装置（カメラ）の３次元位置に置いた複数の「■」（矩形のドット）２０２として示している。
【0033】
すなわち、実際の３Ｄ環境マップ５２２の「データ構造化要素」８０２は、「データ構造化要素」８０２の『■』２０２の位置で取得した車載データごとに、閲覧できる３Ｄマップ要素８０１を纏めるデータである。ただし、データ構造化要素８０２は、３Ｄ環境マップ５２２で必須な要素ではない。
【0034】
図２においては、１２個の３Ｄ環境マップの複数の「３Ｄマップ要素」８０１（『○』２０１）および１２個の３Ｄ環境マップの複数の「データ構造化要素」８０２（『■』２０２）を同時に示している。したがって、複数の『○』２０１および『■』２０２の集合によって、上空俯瞰図における３本の道路１０１、１０２、１０３の位置の概要がわかる。
【0035】
従来技術では、明らかに異なる他２つの道路や反対車線の３Ｄ環境マップを含め、これら同じ地域メッシュに属する１２個の３Ｄ環境マップ群すべてを検索結果として特定取得し、位置姿勢推定を試すことになり、データ取得コスト、および、位置姿勢推定での計算コストがかかる。また、３Ｄ環境マップを順に用いて位置姿勢の算出処理を実行する場合には、最後の１２番目の３Ｄ環境マップになってようやく位置姿勢推定が成功することもあり、リアルタイムでの位置姿勢算出が難しい。なお、実際の地域メッシュは、もっと範囲が広いため（たとえば３次メッシュは一辺約１ｋｍの区間である）、さらに大量の道路と、それに付随して大量の３Ｄ環境マップが存在することになる。
【0036】
図３は、図２と同様に、上空俯瞰図を示しており、図３においては、図２と同様に、図１に示した道路と同じ道路における３Ｄ環境マップを示している。したがって、道路１０１は上りと下りの二車線からなる道路を示しており、道路１０２は、道路１０１と交差する、上りと下りの二車線からなる道路であり、道路１０３は、道路１０１と交差する。上りの一車線（一方通行）からなる道路を示している。
【0037】
さらに、図３においては、図２とは異なり、３Ｄ環境マップの経路情報８０３の車載データの経路情報８３１を示す折れ線矢印３０１～３１２を記載している。これは、作成時の車載データの経路情報であり、データ構造化要素８０２は、図２と同様にある瞬間の車載データの３次元位置『■』２０２で示しているので、この経路情報上の位置となる。
【0038】
具体的には、中折れ線矢印３０１～３０５の５個は、道路１０１の上りにおける車載データの経路情報であり、中折れ線矢印３０６～３０９の４個は、道路１０１の下りにおける車載データの経路情報であり、中折れ線矢印３１０は、道路１０２の上りにおける車載データの経路情報であり、中折れ線矢印３１１は、道路１０２の下りにおける車載データの経路情報であり、中折れ線矢印３１２は、道路１０３の上りにおける車載データの経路情報である。
【0039】
なお、簡単のため、図３で示した３Ｄ環境マップ群の経路情報８０３は、すべて、３Ｄ環境マップ作成時の取得センサのデータ取得方向（撮影方向や測距装置の設置向きなど）が、移動体の移動方向とほぼ等しい（前方カメラや前向き設置した測距装置で取得した）場合の３Ｄ環境マップ群を例として示している。後述するように、後方カメラや後ろ向き設置した測距装置で取得した車載データの場合のように、データ取得方向が移動体の移動方向と逆の場合は、たとえば３Ｄ環境マップの経路情報８０３の向き（図３の中折れ線矢印）の方向は逆とする。もし、道路１０１の上りの３Ｄ環境マップであれば、中折れ線矢印３０１～３０５のような右向き方向ではなく、逆向きの左向き方向になる。このように、データ取得方向が逆の３Ｄ環境マップが混じると、中折れ線の矢印方向が混在して図として分かりにくいため、図３では、データ取得方向が移動方向とほぼ等しいマップ群だけを示した。
【0040】
図４は、本実施の形態にかかる位置姿勢算出方法および位置姿勢算出プログラムの概要の一例を示す説明図である。図４においては、位置姿勢推定部５１５で用いるために、移動経路・移動方向比較部５１３が、位置姿勢推定対象である任意車載データに対する３Ｄ環境マップ５２２を図３の３Ｄ環境マップ群から絞り込む様子を示している。
【0041】
図４において、点線矢印４００は、位置姿勢推定の対象である任意の車載データの走行経路を示している。位置姿勢推定の対象となる車載データの走行経路４００は、車載データと同時に取得したＧＮＳＳ情報を、取得順に並べてベクトル化したものであり、ベクトル方向が車載データの取得装置を搭載した移動体の進行方向を意味する。
【0042】
なお、図４の位置姿勢推定の対象である車載データに対しても、簡単のため、図３および図４のマップ群と同様に、当該車載データのデータ取得方向（撮影方向や測距装置の設置向きなど）も移動体の移動方向とほぼ等しい（前方カメラや前向き設置した測距装置で取得した）ものとする。図４と異なり、データ取得方向が逆の場合は、後述するマップの経路情報と同様に、走行経路４００の向きを反対方向などに補正加工する。
【0043】
点線矢印で示す車載データの走行経路４００と、中折れ線矢印で示すマップ群の経路情報３０１～３１２の移動経路・移動方向を、以下の２つのステップ（「ステップＳ１」：移動経路比較処理、「ステップＳ２」：移動方向比較処理）の処理によって比較をする。
【0044】
（「ステップＳ１」：移動経路比較処理）
まず、「ステップＳ１」において、走行経路４００と各マップ群の経路情報３０１～３１２を比較して、走行経路と経路情報の位置が近く、当該走行経路が一部で重なるか否かを判断する。ここで、マップ３０１、３０５、３０６、３１０、３１１の５個は、いずれも走行経路４００と経路情報が遠くて重ならないことがわかる。したがって、これらのマップ群（３Ｄ環境マップ３０１、３０５、３０６、３１０、３１１）は算出用３Ｄ環境マップとしては選択されないと判断し、それらのマップ群を除外することができる。
まず、「ステップＳ１」において、走行経路４００と各マップ群の経路情報３０１～３１２を比較して、走行経路と経路情報の位置が近く、当該走行経路が一部で重なるか否かを判断する。ここで、マップ３０１、３０５、３０６、３１０、３１１の５個は、いずれも走行経路４００と経路情報が遠くて重ならないことがわかる。したがって、これらのマップ群（３Ｄ環境マップ３０１、３０５、３０６、３１０、３１１）は算出用３Ｄ環境マップとしては選択されないと判断し、それらのマップ群を除外することができる。
【0045】
（「ステップＳ２」：移動方向比較処理）
つぎに、走行経路と経路情報が近く、当該経路情報が一部で重なると判断されて選択されたマップ群３０２～３０４、３０７～３０９、３１２について、「ステップＳ２」において、経路情報の進行方向が走行経路４００の進行方向と等しいか否かを判断する。ここで、マップ３０７、３０８、３０９の３個は、進行方向が走行経路４００と逆方向であることがわかる。また、マップ３１２は、進行方向が走行経路４００に対して略直交していることがわかる。したがって、これらのマップ群（３Ｄ環境マップ３０７、３０８、３０９、３１２）は算出用３Ｄ環境マップとしては選択されないと判断し、それらのマップ群を除外することができる。
つぎに、走行経路と経路情報が近く、当該経路情報が一部で重なると判断されて選択されたマップ群３０２～３０４、３０７～３０９、３１２について、「ステップＳ２」において、経路情報の進行方向が走行経路４００の進行方向と等しいか否かを判断する。ここで、マップ３０７、３０８、３０９の３個は、進行方向が走行経路４００と逆方向であることがわかる。また、マップ３１２は、進行方向が走行経路４００に対して略直交していることがわかる。したがって、これらのマップ群（３Ｄ環境マップ３０７、３０８、３０９、３１２）は算出用３Ｄ環境マップとしては選択されないと判断し、それらのマップ群を除外することができる。
【0046】
その結果、残ったマップ群（３Ｄ環境マップ３０２、３０３、３０４）に絞り込むことができる。その後、絞り込まれたマップ群（３Ｄ環境マップ３０２、３０３、３０４）は、それぞれ、算出用３Ｄ環境マップ（利用対象マップ）として３Ｄ環境マップＤＢ５１０から取得され、位置姿勢推定（算出）に用いられる。
【0047】
そして、取得された算出用３Ｄ環境マップに基づいて、位置姿勢の推定（算出）をおこなう。この位置姿勢推定処理は、取得された位置姿勢算出用の３Ｄ環境マップのデータと、位置姿勢推定対象である処理対象車載データおよびＧＮＳＳ情報を用いて、たとえばＶ－ＳＬＡＭなどを用いて撮影位置姿勢を推定（算出）する。
【0048】
これにより、無用な３Ｄ環境マップのデータ取得コストおよび位置姿勢推定実施コストを削減し、位置姿勢推定完了までの時間を短縮することで、位置姿勢推定をリアルタイムで実施することが可能となる。
【0049】
以下、位置姿勢算出方法および位置姿勢算出プログラムの詳細な内容について、実施の形態１および実施の形態２において説明する。
【0050】
（実施の形態１）
（システム構成例）
図５は、実施の形態１にかかる位置姿勢算出方法を実現するシステム構成の一例を示す説明図である。図５において、実施の形態１にかかる位置姿勢算出方法を実現するシステム（移動体位置姿勢算出システム５００）は、サーバ５０１と、移動体５０３に搭載される車載機５０２とを備える。車載機５０２は、移動体５０３に搭載され、衛星５０５からのＧＮＳＳ情報および車載カメラ（後述する図７に示す撮像装置７０６）の動画像を収集する。
（システム構成例）
図５は、実施の形態１にかかる位置姿勢算出方法を実現するシステム構成の一例を示す説明図である。図５において、実施の形態１にかかる位置姿勢算出方法を実現するシステム（移動体位置姿勢算出システム５００）は、サーバ５０１と、移動体５０３に搭載される車載機５０２とを備える。車載機５０２は、移動体５０３に搭載され、衛星５０５からのＧＮＳＳ情報および車載カメラ（後述する図７に示す撮像装置７０６）の動画像を収集する。
【0051】
そして、サーバ５０１と車載機５０２とが、ネットワーク５０４によって接続されることにより、移動体位置姿勢算出システム５００を構成する。また、移動体位置姿勢算出システム５００は、図示は省略するが、クラウドコンピューティングシステムを用いて、その機能を実現するようにしてもよい。
【0052】
移動体５０３は、具体的には、たとえば、データを収集するコネクテッドカーである。また、一般乗用車やタクシーなどの営業車、二輪車（自動二輪や自転車）、大型車（バスやトラック）などであってもよい。また、移動体５０３は、水上を移動する船舶や上空を移動する航空機、無人航空機（ドローン）、自動走行ロボットなどであってもよい。
【0053】
取得装置の一例である車載機５０２は、車載カメラ（撮像装置７０６）の動画像に関する情報を収集する。また、車載機５０２は、測位情報の一例であるＧＮＳＳ情報を含む移動体５０３の情報を収集する。移動体５０３の情報には、移動体５０３から収集する、移動体５０３の姿勢情報なども含んでもよい。また、車載機５０２は、撮影時刻などに関する情報も収集するようにしてもよい。
【0054】
車載機５０２は、移動体５０３に搭載された専用の装置であってもよく、取り外し可能な機器であってもよい。また車載機５０２は、一般乗用車やタクシーなどの営業車などに搭載されているドライブレコーダーなどであってもよい。さらには、スマートフォンやタブレット端末装置などの通信機能を備えた携帯端末装置を移動体５０３において利用するものであってもよい。また、車載機５０２の各種機能の全部または一部を、移動体５０３が備えている機能を用いて実現するようにしてもよい。
【0055】
したがって、車載機５０２における『車載』という表現は、移動体５０３に搭載されているという意味ではあるが、移動体５０３に搭載された専用装置という意味には限定されない。車載機５０２は、移動体５０３における情報を収集し、収集した情報をサーバ５０１に対して送信できる機能を持った装置であれば、どのような形態の装置であってもよい。
【0056】
車載機５０２は、車載データに関する情報およびＧＮＳＳ情報を含む移動体５０３の情報（車載データ）を取得し、取得した車載データを保存する。そして、保存した車載データを、無線通信によって、ネットワーク５０４を介して、サーバ５０１へ送信する。また、サーバ５０１から配信されたプログラムを含む各種データを、ネットワーク５０４を介して、無線通信により受信する。
【0057】
また、車載機５０２は、近距離通信機能により、近くを走行中の別の移動体５０３の情報を取得し、サーバ５０１へ送信するようにしてもよい。また、車載機５０２どうしが、近距離通信機能により、通信をおこない、他の車載機５０２を介して、サーバ５０１との通信をおこなうようにしてもよい。
【0058】
このようにして、移動体位置姿勢算出システム５００において、サーバ５０１は、移動体５０３に搭載された車載機５０２から車載データを取得するとともに、各車載機５０２へ各種データを配信することができる。
【0059】
また、車載機５０２は、通信手段を備えていなくてよい。すなわち、車載機５０２は、サーバ５０１とは、ネットワーク５０４を介して接続されていなくてもよい。その場合は、車載機５０２に蓄積されたデータは、オフラインで（たとえば、記録メディアを介して人手などにより）、サーバ５０１に入力することができる。
【0060】
サーバ５０１は、環境マップ作成部５１１、マップ登録部５１２、移動経路・移動方向比較部５１３、マップ取得部５１４、位置姿勢推定部５１５を有する。また、サーバ５０１は、車載データおよびＧＮＳＳ情報（「車載データ＋ＧＮＳＳ情報」５２１）を持つ。また、サーバ５０１は、内部処理データとして、３Ｄ環境マップＤＢ５１０を有していてもよい。
【0061】
車載データは、位置姿勢推定部５１５が３Ｄ環境マップ５２２を使って位置姿勢推定処理をおこなう対象となる車載データである。具体的には、移動体の装置で取得した移動体周辺に関するデータであり、たとえば、車載カメラの動画像、車載ＬｉＤＡＲ装置などによる周辺測距データ、などであってもよい。
【0062】
なお、環境マップ作成部５１１が入力するデータ（３Ｄ環境マップ作成用のデータ）と、移動経路・移動方向比較部５１３および位置姿勢推定部５１５が用いるデータ（３Ｄ環境マップを使って位置姿勢推定をおこなう対象のデータ）とは、ここでは異なる移動体の異なるシーンの車載データとして説明するが、同じ移動体および同じシーンの車載データであってもよい。
【0063】
ＧＮＳＳ情報は、移動体５０３の測位位置データであり、車載データと同時に取得することができる。たとえば、一般車のＧＰＳ情報であってもよいし、それ以外の手段で取得した位置情報であってもよい。また、緯度経度値だけではなく、標高などの高さ値を含めるようにしてもよい。
【0064】
環境マップ作成部５１１は、「車載データ＋ＧＮＳＳ情報」５２１を入力し、その「車載データ＋ＧＮＳＳ情報」５２１から３Ｄ（３次元）環境マップ５２２を作成する。
【0065】
マップ登録部５１２は、環境マップ作成部５１１において作成された３Ｄ環境マップ５２２を３Ｄ環境マップＤＢ５１０に登録する。その際、検索用のインデックスを作成してもよい。たとえば、任意のデータベース項目を検索用の別テーブルとして保持するようにしてもよい。インデックスを用いることによって、３Ｄ環境マップＤＢ５１０を、より高速に検索することができる。
【0066】
移動経路・移動方向比較部５１３は、移動体５０３または当該移動体５０３に搭載された取得装置の一例である車載機５０２が３Ｄ環境マップ作成用のデータを取得する際の当該取得位置を含む複数の３Ｄ環境マップ５２２の経路情報（後述する図８に示す経路情報８０３）と、位置姿勢の算出の対象となる対象移動体５０３または当該対象移動体に搭載された取得装置（車載機５０２）から取得した経路情報との比較をおこなう。
【0067】
マップ取得部５１４は、移動経路・移動方向比較部５１３による比較の結果に基づいて、３Ｄ環境マップＤＢ５１０に登録されている複数の環境マップの中から、対象移動体５０３または当該対象移動体５０３に搭載された取得装置（車載機５０２）がデータを取得する際の取得位置姿勢の算出に用いる算出用環境マップを特定（取得）する。
【0068】
ここで、算出用環境マップは、対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報（図４に示した車載データ走行経路４００）のうちの移動経路および移動方向の少なくともいずれかが類似する経路情報を有する３Ｄ環境マップ（図３および図４に示したマップ群３０２～３０４、３０７～３１２）であってもよい。
【0069】
また、算出用環境マップは、対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報（車載データ走行経路４００）のうちの移動経路および移動方向が類似する経路情報を有する３Ｄ環境マップ（図３および図４に示したマップ群３０２～３０４）であってもよい。
【0070】
また、移動経路・移動方向比較部５１３、マップ取得部５１４は、移動経路および移動方向が、取得装置の種類、前記取得装置の設置位置、取得装置が取得するデータの取得方向の少なくともいずれか一つ、具体的には、たとえば、取得装置が撮影装置である場合に、当該撮影装置が後ろ向きカメラである場合に、後述する図９のフローチャートに示すように、その各種情報に基づいて移動経路および移動方向を補正するようにしてもよい。
【0071】
また、移動経路・移動方向比較部５１３、マップ取得部５１４は、環境マップの経路情報、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得したデータ（図２～図４に示した３Ｄマップ要素２０１）の各取得位置を中心とした任意の大きさの２次元または３次元の所定形状（たとえば、円、楕円、球、楕円体など）を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいて、算出用３Ｄ環境マップ５２２を特定するようにしてもよい。
【0072】
また、移動経路・移動方向比較部５１３、マップ取得部５１４は、環境マップの経路情報、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報における経路（後述する図１０における矢印１０００）を含む任意の大きさ（図１０におけるＧＰＳ誤差１００１）の２次元または３次元の所定形状１００２を設定し、当該所定形状１００２の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいて、算出用環境マップを特定するようにしてもよい。
【0073】
また、移動経路・移動方向比較部５１３、マップ取得部５１４は、複数の算出用３Ｄ環境マップ５２２が特定された場合に、経路の重なり度に基づいて、特定された算出用３Ｄ環境マップ５２２の利用優先度を算出するようにしてもよい。
【0074】
位置姿勢推定部５１５は、マップ取得部５１４によって取得された算出用環境マップを用いて、対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の取得位置姿勢（「推定位置姿勢」５２３）を算出する。
【0075】
図５においては、サーバ５０１が、環境マップ作成部５１１と、マップ登録部５１２と、移動経路・移動方向比較部５１３と、マップ取得部５１４と、位置姿勢推定部５１５と、を有する構成とした。図示は省略するが、これらの各機能部の少なくとも一つを、サーバ５０１に加えて、あるいは、サーバ５０１に代えて、車載機５０２が有するようにしてもよい。車載機５０２が、各機能部５１１、５１２、５１３、５１４、５１５の少なくとも一つを有する場合は、サーバ５０１が実施する処理の内容と同じであってもよい。また、サーバ５０１は複数のサーバから構成されてもよく、各機能部を分散して処理をおこなってもよい。
【0076】
（サーバのハードウェア構成例）
図６は、サーバのハードウェア構成の一例を示すブロック図である。情報処理装置の一例であるサーバ５０１は、１つ以上のＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）６０１と、メモリ６０２と、ネットワークＩ／Ｆ（Ｉｎｔｅｒｆａｃｅ）６０３と、記録媒体Ｉ／Ｆ６０４と、記録媒体６０５と、を有する。また、各構成部は、バス６００によってそれぞれ接続される。
図６は、サーバのハードウェア構成の一例を示すブロック図である。情報処理装置の一例であるサーバ５０１は、１つ以上のＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）６０１と、メモリ６０２と、ネットワークＩ／Ｆ（Ｉｎｔｅｒｆａｃｅ）６０３と、記録媒体Ｉ／Ｆ６０４と、記録媒体６０５と、を有する。また、各構成部は、バス６００によってそれぞれ接続される。
【0077】
ここで、ＣＰＵ６０１は、サーバ５０１の全体の制御を司る。メモリ６０２は、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）およびフラッシュＲＯＭなどを有する。具体的には、たとえば、フラッシュＲＯＭやＲＯＭが各種プログラムを記憶し、ＲＡＭがＣＰＵ６０１のワークエリアとして使用される。メモリ６０２に記憶されるプログラムは、ＣＰＵ６０１にロードされることで、コーディングされている処理をＣＰＵ６０１に実行させる。
【0078】
ネットワークＩ／Ｆ６０３は、通信回線を通じてネットワーク５０４に接続され、ネットワーク５０４を介して他の装置（たとえば、車載機５０２や、他のサーバやシステム）に接続される。そして、ネットワークＩ／Ｆ６０３は、ネットワーク５０４と自装置内部とのインターフェースを司り、他の装置からのデータの入出力を制御する。ネットワークＩ／Ｆ６０３には、たとえば、モデムやＬＡＮアダプタなどを採用することができる。
【0079】
記録媒体Ｉ／Ｆ６０４は、ＣＰＵ６０１の制御にしたがって記録媒体６０５に対するデータのリード／ライトを制御する。記録媒体６０５は、記録媒体Ｉ／Ｆ６０４の制御で書き込まれたデータを記憶する。記録媒体６０５としては、たとえば、磁気ディスク、光ディスクなどが挙げられる。
【0080】
なお、サーバ５０１は、上述した構成部のほかに、たとえば、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）、キーボード、ポインティングデバイス、ディスプレイなどを有していてもよい。
【0081】
（車載機のハードウェア構成例）
図７は、車載機のハードウェア構成の一例を示すブロック図である。情報収集装置の一例である車載機５０２は、ＣＰＵ７０１と、メモリ７０２と、無線通信装置７０３と、移動体Ｉ／Ｆ７０４と、受信装置７０５と、撮像装置（測距装置）７０６と、を有する。また、各構成部は、バス７００によってそれぞれ接続される。
図７は、車載機のハードウェア構成の一例を示すブロック図である。情報収集装置の一例である車載機５０２は、ＣＰＵ７０１と、メモリ７０２と、無線通信装置７０３と、移動体Ｉ／Ｆ７０４と、受信装置７０５と、撮像装置（測距装置）７０６と、を有する。また、各構成部は、バス７００によってそれぞれ接続される。
【0082】
ＣＰＵ７０１は、車載機５０２の全体の制御を司る。メモリ７０２は、たとえば、ＲＯＭ、ＲＡＭおよびフラッシュＲＯＭなどを有する。具体的には、たとえば、フラッシュＲＯＭやＲＯＭが各種プログラムを記憶し、ＲＡＭがＣＰＵ７０１のワークエリアとして使用される。メモリ７０２に記憶されるプログラムは、ＣＰＵ７０１にロードされることで、コーディングされている処理をＣＰＵ７０１に実行させる。
【0083】
無線通信装置７０３は、発信された電波を受信したり、電波を発信したりする。アンテナと受信装置とを含む構成であり、各種通信規格による移動通信（具体的には、たとえば、３Ｇ、４Ｇ、５Ｇ、ＰＨＳ通信など）、Ｗｉ－Ｆｉ（登録商標）などの通信を送受信する機能を備えている。
【0084】
移動体Ｉ／Ｆ７０４は、移動体５０３と車載機５０２の自装置内部とのインターフェースを司り、移動体５０３からのデータの入出力を制御する。したがって、車載機５０２は、たとえば、移動体Ｉ／Ｆ７０４を介して移動体５０３が備えるＥＣＵ（各種センサなどを含む）７０７から情報を収集することができる。移動体Ｉ／Ｆ７０４は、具体的には、たとえば、有線により接続する際に用いるコネクタや近距離無線通信（具体的には、たとえば、Ｂｌｕｅｔｏｏｔｈ（登録商標））装置などであってもよい。
【0085】
受信装置（たとえばＧＰＳ受信装置などのＧＮＳＳ受信装置）７０５は、複数の衛星５０５からの電波を受信し、受信した電波に含まれる情報から、地球上の現在位置を算出する。
【0086】
撮像装置（たとえばカメラなど）７０６は、静止画や動画を撮像し、画像情報として出力する機器である。具体的には、たとえば、レンズと撮像素子とを備える構成である。具体的には、単眼カメラの映像以外に、複数カメラ（ステレオカメラ）の画像対などを含む。このように、撮影装置７０６は、映像（動画）や単画像（静止画）を含む動画像からなる画像情報を取得することができる。また、撮像装置７０６は、一般乗用車やタクシーなどの営業車に搭載されるドライブレコーダーなどであってもよい。
【0087】
撮像装置７０６による撮像画像は、メモリ７０２に保存される。また、カメラなどの撮像装置７０６は、画像認識機能や、バーコードやＱＲコード（登録商標）を読み取る機能や、ＯＭＲ（Ｏｐｔｉｃａｌ Ｍａｒｋ Ｒｅａｄｅｒ）、ＯＣＲ（Ｏｐｔｉｃａｌ Ｃｈａｒａｃｔｅｒ Ｒｅａｄｅｒ）機能などを備えていてもよい。
【0088】
また、撮影装置７０６は、測距装置であってもよい。測距装置７０６は、レーザー光を照射し、物体に当たって跳ね返ってくるまでの時間を計測し、物体までの距離や方向を測定する。具体的には、ＬｉＤＡＲユニットなどによって、その機能を実現することができる。
【0089】
図７に示したように、ＧＮＳＳ受信装置７０５および撮像装置７０６は、車載機５０２が備えていてもよく、また、移動体５０３が備えていたり、別途、外付けされたものを用いるようにしてもよい。その際、ＧＮＳＳ受信装置７０５あるいは撮像装置７０６と、車載機５０２とのデータのやりとりは、有線または無線通信によりおこなうようにしてもよい。
【0090】
ＧＮＳＳ受信装置７０５や撮像装置７０６を、車載機５０２が備えていない場合は、移動体Ｉ／Ｆ７０４などを介して、それらの情報を取得するようにしてもよい。また、車載機５０２は、図示は省略するが、各種入力装置、ディスプレイ、メモリカードなどの記録媒体の読み書き用のインターフェース、各種入力端子などを備えていてもよい。
【0091】
（３Ｄ環境マップの内容）
図８は、実施の形態１にかかる３Ｄ環境マップの構成の一例を示す説明図である。図８において、３Ｄ環境マップ５２２（５２２ａ～５２２ｃ）は、３Ｄマップ要素（３Ｄ画像特徴要素）８０１と、データ構造化要素８０２と、経路情報８０３と、を有する。
図８は、実施の形態１にかかる３Ｄ環境マップの構成の一例を示す説明図である。図８において、３Ｄ環境マップ５２２（５２２ａ～５２２ｃ）は、３Ｄマップ要素（３Ｄ画像特徴要素）８０１と、データ構造化要素８０２と、経路情報８０３と、を有する。
【0092】
また、３Ｄ環境マップ５２２は、図示は省略するが、保持するデータ構成要素の、３Ｄマップ要素８０１や、データ構造化要素８０２、経路情報８０３、のそれぞれに対し、各要素や情報を識別する一意の識別情報である「ＩＤ」情報を有していてもよい。なお、これらの３Ｄ環境マップ５２２のデータ構造化要素８０２は、精度のよい位置姿勢推定をおこなうためのものであり、省略することも可能である。
【0093】
３Ｄマップ要素８０１は、位置姿勢推定（算出）処理で用いる車載データの特徴要素に関するデータである。たとえば、図８は、具体的な位置姿勢推定で用いる車載データの例として、車載データが画像情報（動画像）の場合を示しており、特徴要素は画像特徴であり、３Ｄマップ要素８０１は３Ｄ画像特徴である。また、３Ｄマップ要素（３Ｄ画像特徴要素）８０１は、「３次元実座標位置」８１１と「（特徴要素を特定するための）特定情報」８１２を含むが、それぞれ、画像特徴の３次元実座標位置であり、画像特徴を特定するための特定情報である。具体的な画像特徴としては、任意の画像処理手法で車載データの画像から抽出できる画像特徴点や画像特徴線などが考えられる。
【0094】
なお、車載データとしてＬｉＤＡＲデータを用いる場合には、３Ｄマップ要素８０１は３ＤＬｉＤＡＲ特徴要素となり、ＬｉＤＡＲ特徴要素としては、ＬｉＤＡＲ測距データからスキャンニング角度などを用いた任意の解析処理手法で抽出できる３次元点群、同３次元点群から求めた平面群（３Ｄメッシュ群）や微小立体群（３Ｄボクセル群）などが考えられる。
【0095】
３Ｄマップ要素８０１の実座標３次元位置である「３次元実座標位置」８１１は、特徴要素が画像特徴の場合は、Ｖ－ＳＬＡＭなどの処理内で、同じ特徴要素が複数時刻の車載データ内に出現する時のデータ内位置の変化などを用いて、既知の三角測量や微小位置変化に対する最適化計算などを使って算出することができる。また、特徴要素がＬｉＤＡＲ特徴要素の場合は、ＬｉＤＡＲの測距データの３次元点群の３Ｄ座標そのものを使ってもよいし、ＬｉＤＡＲデータから既知の物体復元手法などの解析処理手法を用いて、近傍の３次元点群を繋いだ３Ｄ平面である３Ｄメッシュ群や点群の存在微小立体空間として作成した３Ｄボクセル群などの特徴要素を抽出し、それらの３次元位置を使ってもよい。
【0096】
また、「特定情報」８１２は、特徴要素を特定するための情報であり、特徴要素を抽出した時の任意の属性情報である。たとえば画像特徴であれば画像特徴量などであってもよい。また、ＬｉＤＡＲであれば、各３次元点周辺の３Ｄ点群密度や、または各３Ｄ平面（３Ｄメッシュ）の法線ベクトル、各３Ｄボクセル内の３Ｄ点群密度などであってもよい。さらに、ＬｉＤＡＲデータに加えて同時撮影した動画像がある場合は、当該動画像から抽出したＬｉＤＡＲ３次元点に対応する画素色などであってもよい。このように、特徴要素どうしの比較および対応付けに使う属性要素であればよい。
【0097】
位置姿勢推定部５１５は、３Ｄ環境マップ５２２内の３次元実座標位置８１１が既知である各３Ｄマップ要素８０１群が、位置姿勢推定対象の車載データ（動画像あるいはＬｉＤＡＲ測距結果）内に実際に現れた位置と、推定中の位置姿勢値から幾何的に計算できる車載データ内に現れるはずの位置とをそれぞれ比較する。そして、実際の位置と現れるはずの位置の差が少なくなるように最適化をおこなうことで、車載データ装置（カメラあるいはＬｉＤＡＲ）の（撮影あるいは測距）位置姿勢を推定することができる。このとき、多数の３Ｄマップ要素８０１群内から、位置姿勢推定対象の車載データから実際に抽出した特徴と等しいものを、比較対象としてを特定する必要があり、「特定情報」８１２は、この特定に用いることができる。
【0098】
「特定情報」８１２が画像特徴量である場合は、当該画像特徴量は、一般的な画像処理で画像特徴を抽出する際に算出することができる。たとえば、画像特徴量として、画像特徴となる画素と周辺画素との輝度差状況をバイナリ値に変換した値などであってもよい。したがって、画像特徴量は、利用する画像処理の種類（画像処理で抽出する画像特徴の種類）に依存する。一般的に同じ画像特徴種類の画像特徴量どうしは比較が可能なため、任意の画像特徴量の比較方法を通して、３Ｄ環境マップ５２２内で、位置姿勢推定対象の画像から抽出した画像特徴に対応する特定情報８１２を持つ３Ｄ画像特徴要素８０１を特定することができる。
【0099】
データ構造化要素８０２は、３Ｄマップ要素８０１を纏めるもので、たとえば３Ｄマップ要素８０１群を纏めるグループのようなものであってもよい。データ構造化要素８０２は、主に３Ｄ環境マップ５２２を位置姿勢推定部５１５で用いる際に、位置姿勢推定対象である車載データに関係しそうな３Ｄマップ要素８０１を、大まかに選別するのに用いることができる。
【0100】
一般的にロバストで正確な位置姿勢推定（算出）をおこなうために、３Ｄ環境マップ５２２は多数の３Ｄマップ要素８０１を保持する。しかし、多数の３Ｄマップ要素８０１のどれが位置姿勢推定に使えるのかを、３Ｄマップ要素８０１の「特定情報（特徴要素を特定するための特定情報）」８１２を使って比較していたのでは、効率が悪い。また、比較対象が多すぎると、同じ環境マップ５２２内に同じ特定情報８１２を持つ３Ｄマップ要素が複数存在してしまうことも考えられ、比較によって誤った３Ｄマップ要素を特定してしまう可能性も高くなる。これに対し、あらかじめ車載データを取得する瞬間ごとに、同時に参照できた３Ｄマップ要素８０１を纏めた「データ構造化要素」８０２を用意しておくことで、間違った３Ｄマップ要素８０１を使ってしまうことを防止することができる。
【0101】
たとえば、車載データとして動画像を用いる場合には、３Ｄ環境マップ作成に使った各画像をデータ構造化要素８０２とする。動画像内の全画像をデータ構造化要素８０２にしてもよい。特に動画像内で見え変化が大きい重要な画像（「キーフレーム」と呼ぶ）のみをデータ構造化要素８０２にしてもよい。このとき、３Ｄマップ要素（３Ｄ画像特徴要素）８０１群の中から各画像で抽出したものだけを、当該画像であるデータ構造化要素８０２の「関係する３Ｄマップ要素群」８２１として纏めて保持する。
【0102】
同様に、３Ｄ環境マップ５２２の作成に用いたＬｉＤＡＲ測距データの各時刻で、その時参照できたＬｉＤＡＲ特徴要素（３次元点群や、３Ｄメッシュ群、３Ｄボクセル群など）を纏めて、「関係する３Ｄマップ要素群」８２１として保持してもよい。
【0103】
あるいは、データ構造化要素８０２として、３Ｄマップ要素８０１が属する物体（地物）をとらえ、同じ地物に属する３Ｄマップ要素８０１群を保持するようにしてもよい。同じ物体かどうかは、３次元実座標位置８１１が近いものを纏めてもよいし、任意の画像認識などの認識手段で車載データ内の物体領域を特定して該領域内の特徴要素か否かから判断してもよいし、点群密度や法線ベクトルなどの特定情報が似ているかどうかから判断してもよい。このように、物体単位で纏めることで、似た存在位置の３Ｄマップ要素８０１を纏めることになり、同時に閲覧しやすい３Ｄマップ要素８０１をある程度絞り込むことができる。
【0104】
一般的な３Ｄ環境マップとは異なり、実施の形態１にかかる３Ｄ環境マップ５２２は、さらに経路情報８０３を保持する。これは、３Ｄ環境マップ５２２を作成するときに用いた「車載データの経路情報」８３１を含む。この「車載データの経路情報」８３１は、たとえば、車載データと同時取得したＧＮＳＳ情報を取得順に並べたものであってもよい。また、「車載データの経路情報」８３１は、実際には、ＧＮＳＳ情報の値をそのまま保持してもよいし、測位失敗部分の補間や、任意のフィルタを用いてノイズ除去をした情報を保持してもよい。
【0105】
また、「車載データの経路情報」８３１は、データ構造化要素８０２に合わせた位置情報に加工したものであってもよい。たとえば、データ構造化要素８０２が画像キーフレームの場合は、一般的なＧＰＳ測位センサ（一般的に１Ｈｚ）と映像（たとえば３０ｆｐｓ、すなわち３０Ｈｚ相当）では取得間隔が異なるため、ＧＮＳＳ情報を画像キーフレームの取得時刻に相当する位置情報に加工して保持するようにしてもよい。
【0106】
また、「車載データの経路情報」８３１は、経路の各位置（実座標位置）と、その通過順または各位置の通過時刻を保持してもよい。ここで、通過時刻は絶対時刻ではなく、車載データ取得開始時からの相対時刻でもよい。これらの通過順または通過時刻を用いて、各位置の次の位置（次の時刻の位置）へ向けた向き（ベクトル）を求めることによって、簡易的に各位置での進行方向情報を得ることができる。また、「車載データの経路情報」８３１は、経路の位置（実座標位置）および通過順（各位置の通過時刻）の代わり、またはそれらに加えて、経路の各位置での進行方向情報を保持するようにしてもよい。
【0107】
さらに、「車載データの経路情報」８３１は、車載データの取得センサの移動体内の設置位置が既知であれば、設置位置によってＧＮＳＳの経路情報を加工したものを使ってもよい。たとえば、ＧＮＳＳ情報による経路情報の位置を、「ＧＮＳＳ装置の移動体内設置位置」－「車載データ取得装置の移動体内設置位置」の位置差分を加えた位置としてもよい。これによって、より正確に車載データに対応する経路とすることができる。
【0108】
また、「車載データの経路情報」８３１は、車載データの取得センサが移動体外のどの方向の状況を取得するよう搭載されているかの情報が既知であれば、当該情報によってＧＮＳＳの経路情報を加工したものを使うようにしてもよい。たとえば、車載データが移動体の進行方向と１８０度反対方向に向けたカメラ、すなわち移動体の後方を撮影する後方カメラで取得したデータだった場合には、ＧＮＳＳの経路情報から得た移動方向を１８０度反対方向に向けた方向を、「車載データの経路情報」８３１の移動方向とすることができる。
【0109】
このように、取得装置のデータ取得方向（撮影方向など）となるよう補正加工した場合には、厳密は移動方向ではないが、簡単のため、以後はどちらのケースでも移動方向とするものとする。
【0110】
なお、後方カメラ・前方カメラのような、車載データの取得装置（センサ）の種類から、あらかじめ設置位置や取得方向が特定される場合に、それらのセンサ種類を車載データと共に取得して、自動的に「車載データの経路情報」８３１の位置や移動方向を加工修正するようにしてもよい。
【0111】
「車載データの経路情報」８３１には、これら車載データの取得センサの位置や取得方向にともない、実際に位置や移動方向を加工した値を保持するようにしてもよい。また、「車載データの経路情報」８３１において、加工するためのデータを保持して、経路位置や移動方向が必要なときに値を加工して使うようにしてもよい。
【0112】
なお、ＳＬＡＭを使って３Ｄ環境マップ５２２を作成する場合には、車載データの各時刻データ（動画像の場合は各画像）の位置姿勢推定と３Ｄ環境マップ作成を同時に実施するため、作成時に使ったＧＮＳＳ情報またはＧＮＳＳ情報を加工した情報を「車載データの経路情報」８３１とするのではなく、３Ｄ環境マップ作成時に同時推定した車載装置の位置姿勢の位置や姿勢を「車載データの経路情報」８３１の位置および進行方向（移動方向）として保持するようにしてもよい。
【0113】
この推定位置姿勢の位置姿勢値を使うことで、疎な取得間隔のＧＮＳＳ情報と車載データの取得間隔を考慮した加工を実施する処理を不要にすることができ、ＧＮＳＳによる経路情報より高精度で正確な経路情報を保持管理できる。
【0114】
さらに、３Ｄ環境マップ５２２はその他の情報として、「車載データの経路情報」８３１のほかに、作成時の車載データの取得時の情報、たとえば車載データの取得年月日や取得時刻、取得車両種類などの情報をさらに保持するようにしてもよい。
【0115】
（環境マップ作成部５１１の内容）
図５に示した環境マップ作成部５１１は、「車載データ＋ＧＮＳＳ情報」５２１を入力として、位置姿勢推定部５１５において用いるための３Ｄ環境マップ５２２を、任意の既知の方法で作成する。たとえば、車載データがＬｉＤＡＲデータである場合は、ＬｉＤＡＲ情報の測距装置からの距離、すなわち測距センサからの相対３次元位置（３Ｄ位置）が既知な測距点群をＬｉＤＡＲ特徴要素としてもよい。あるいは、測距による相対３次元位置を持つ点群データから既知の物体復元手法などの解析処理手法を用いて、近傍点群を繋いだ３Ｄ平面である３Ｄメッシュ群や点群の存在する微小立体空間を抽出した３Ｄボクセル群などを、ＬｉＤＡＲ特徴要素としてもよい。
図５に示した環境マップ作成部５１１は、「車載データ＋ＧＮＳＳ情報」５２１を入力として、位置姿勢推定部５１５において用いるための３Ｄ環境マップ５２２を、任意の既知の方法で作成する。たとえば、車載データがＬｉＤＡＲデータである場合は、ＬｉＤＡＲ情報の測距装置からの距離、すなわち測距センサからの相対３次元位置（３Ｄ位置）が既知な測距点群をＬｉＤＡＲ特徴要素としてもよい。あるいは、測距による相対３次元位置を持つ点群データから既知の物体復元手法などの解析処理手法を用いて、近傍点群を繋いだ３Ｄ平面である３Ｄメッシュ群や点群の存在する微小立体空間を抽出した３Ｄボクセル群などを、ＬｉＤＡＲ特徴要素としてもよい。
【0116】
これらＬｉＤＡＲ特徴要素の３Ｄ位置は、ＬｉＤＡＲデータ（ＬｉＤＡＲ特徴要素）を用いたＳＬＡＭなどの時系列解析をおこなうことで、さらにより正確な３Ｄ位置とすることができる。ＧＮＳＳデータによってこれら３Ｄ位置を実座標系の緯度経度値に変換して算出し、３Ｄ環境マップ５１１とすることができる。
【0117】
また、車載データが動画像データである場合は、動画像を用いたＶ－ＳＬＡＭを実施して、画像特徴要素の３Ｄ位置を求め、ＧＮＳＳ情報によって相対位置から実座標系での緯度経度値に変換して算出し、３Ｄ環境マップ５２２とすることができる。
【0118】
このとき、実座標系への変換を３Ｄ位置算出と同時に実施してもよいし、あるいは、ＬｉＤＡＲ情報の特徴要素、または動画像の画像特徴の３Ｄ位置は相対値として保持して、実座標系への変換方法、たとえば座標系変換行列の値などを別途保持していてもよい。
【0119】
環境マップ作成部５１１において作成した３Ｄ環境マップ５２２を用いて位置姿勢推定部５１５で位置姿勢を求めるため、一般的に、環境マップ作成部５１１と位置姿勢推定部５１５で用いる３Ｄ位置つきの特徴要素（３Ｄ環境マップ５２２の３Ｄマップ要素（３Ｄ画像特徴要素）８０１）は、同じである必要がある。
【0120】
また、環境マップ作成部５１１は、従来の３Ｄ環境マップ（図８の３Ｄマップ要素８０１とデータ構造化要素８０２）以外に、新たに経路情報８０３を作成する。なお、経路情報８０３は、便宜的に環境マップ作成部５１１が算出するものとしたが、続く、マップ登録部５１２において３Ｄ環境マップ５２２を登録する際に算出するようにしてもよい。
【0121】
環境マップ作成部５１１は、具体的には、たとえば、図５に示したサーバ５０１において、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した車載機５０２において、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。
【0122】
（マップ登録部５１２の内容）
マップ登録部は５１２、環境マップ作成部５１１において作成された、３Ｄマップ要素８０１、データ構造化要素８０２、経路情報８０３を含む（なお、データ構造化要素８０２は省略可能）３Ｄ環境マップ５２２を、移動経路・移動方向比較部５１３で比較特定し、また、位置姿勢推定部５１５で簡単に引用できるように、３Ｄ環境マップＤＢ５１０などに登録管理することをおこなう。
マップ登録部は５１２、環境マップ作成部５１１において作成された、３Ｄマップ要素８０１、データ構造化要素８０２、経路情報８０３を含む（なお、データ構造化要素８０２は省略可能）３Ｄ環境マップ５２２を、移動経路・移動方向比較部５１３で比較特定し、また、位置姿勢推定部５１５で簡単に引用できるように、３Ｄ環境マップＤＢ５１０などに登録管理することをおこなう。
【0123】
そのために、上述したように、３Ｄ環境マップＤＢ５１０内で検索にかかわる要素を用いて検索専用の別テーブルを作成するようにしてもよい。これにより、検索速度向上が図れることから、あらかじめ３Ｄ環境マップをＤＢ５１０登録する際に、データベースの一般的な既知のＩｎｄｅｘまたは空間Ｉｎｄｅｘ情報の作成手段を用いて、検索用Ｉｎｄｅｘとして経路情報８０３の一部を用いた検索用のＩｎｄｅｘ情報または空間Ｉｎｄｅｘ情報をあらかじめ準備しておくようにしてもよい。このようにすることで、移動経路・移動方向比較部５１３の比較を高速化することが可能となる。
【0124】
マップ登録部５１２は、具体的には、たとえば、図６に示したサーバ５０１において、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、あるいは、ネットワークＩ／Ｆ６０３、記録媒体Ｉ／Ｆ６０４などによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した車載機５０２において、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、あるいは、無線通信装置７０３などによって、その機能を実現するようにしてもよい。
【0125】
（環境マップ作成部５１１、マップ登録部５１２の処理手順）
図９は、実施の形態１にかかる環境マップ作成部およびマップ登録部の処理の一例を示すフローチャートである。図９のフローチャートにおいて、入力車載データとＧＮＳＳ情報とから、３Ｄ環境マップ５２２、より具体的には、３Ｄ環境マップ５２２の３Ｄ画像特徴８０１を作成する（ステップＳ９０１）。
図９は、実施の形態１にかかる環境マップ作成部およびマップ登録部の処理の一例を示すフローチャートである。図９のフローチャートにおいて、入力車載データとＧＮＳＳ情報とから、３Ｄ環境マップ５２２、より具体的には、３Ｄ環境マップ５２２の３Ｄ画像特徴８０１を作成する（ステップＳ９０１）。
【0126】
そして、３Ｄ環境マップ５２２（３Ｄ画像特徴８０１）が作成できて有ったか否かを判断する（ステップＳ９０２）。ここで、３Ｄ環境マップ５２２（３Ｄ画像特徴８０１）が作成できず、なかった場合（ステップＳ９０２：Ｎｏ）は、何もせずに、一連の処理を終了する。
【0127】
一方、３Ｄ環境マップ５２２（３Ｄ画像特徴８０１）が作成できて有った場合（ステップＳ９０２：Ｙｅｓ）は、つぎに、マップ作成時の車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知であり、車載データの経路情報８３１を加工する必要があるか否かを判断する（ステップＳ９０３）。
【0128】
ここで、マップ作成時の車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知であり、車載データの経路情報８３１を加工する必要がある場合（ステップＳ９０３：Ｙｅｓ）は、マップ作成時の車載データの経路情報８３１と、車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかを用いて、３Ｄ環境マップ５２２の経路情報８０３を作成する（ステップＳ９０４）。その後、ステップＳ９０６へ移行する。
【0129】
一方、マップ作成時の車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知ではないか、車載データの経路情報８３１を加工する必要がない場合（ステップＳ９０３：Ｎｏ）は、マップ作成時の車載データの経路情報８３１から、３Ｄ環境マップ５２２の経路情報８０３を作成する（ステップＳ９０５）。その後、ステップＳ９０６へ移行する。ここまでの処理（ステップＳ９０１～Ｓ９０５の各処理）は、たとえば、環境マップ作成部５１１によっておこなうことができる。
【0130】
その後、経路情報８０３を含む３Ｄ環境マップ５２２を、３Ｄ環境マップＤＢ５１０に登録する（ステップＳ９０６）。これにより、一連の処理を終了する。このステップＳ９０６の処理は、たとえば、マップ登録部５１２によっておこなうことができる。
【0131】
このようなステップＳ９０１～Ｓ９０６の各処理の手順によって、車載データおよびＧＮＳＳ情報（「車載データ＋ＧＮＳＳ情報」５２１）から３Ｄ環境マップ５２２を作成し、作成された３Ｄ環境マップ５２２を３Ｄ環境マップＤＢ５１０に登録することができる。
【0132】
（移動経路・移動方向比較部５１３の内容）
移動経路・移動方向比較部５１３は、位置姿勢を推定したい車載データおよび車載データと同時取得した、ＧＰＳなどの測位情報であるＧＮＳＳ情報（「車載データ＋ＧＮＳＳ情報」５２１）を入力し、当該車載データに関係する３Ｄ環境マップ群を、マップ登録部５１２が登録済の３Ｄ環境マップＤＢ５１０から特定する。このとき、３Ｄ環境マップ５２２の経路情報８０３を、車載データセンサの設置位置や、取得方向、センサ種類などから補正加工したように、推定したい車載データのＧＮＳＳ情報も加工するようにしてもよい。
移動経路・移動方向比較部５１３は、位置姿勢を推定したい車載データおよび車載データと同時取得した、ＧＰＳなどの測位情報であるＧＮＳＳ情報（「車載データ＋ＧＮＳＳ情報」５２１）を入力し、当該車載データに関係する３Ｄ環境マップ群を、マップ登録部５１２が登録済の３Ｄ環境マップＤＢ５１０から特定する。このとき、３Ｄ環境マップ５２２の経路情報８０３を、車載データセンサの設置位置や、取得方向、センサ種類などから補正加工したように、推定したい車載データのＧＮＳＳ情報も加工するようにしてもよい。
【0133】
移動経路・移動方向比較部５１３は、具体的には、たとえば、図５に示したサーバ５０１において、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した車載機５０２において、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。
【0134】
移動経路・移動方向比較部５１３は、具体的には、以下の手順によって、当該車載データに関係する３Ｄ環境マップ群を特定（絞り込み）することができる。
【0135】
移動経路・移動方向比較部５１３は、まず、３Ｄ環境マップＤＢ５１０内の３Ｄ環境マップ５２２群から、車載データの走行経路４００の各位置と、３Ｄ環境マップの関連情報の経路の位置が近いものを選定する。たとえば、２つの経路を構成する各３Ｄ位置点どうしで、他方の点と重なるかどうかを判定してもよい。
【0136】
このとき、車載データの走行経路４００や、３Ｄ環境マップ５２２の関連情報の経路の実座標３Ｄ位置には、測位装置による位置誤差が存在することを想定する。そして、それぞれを構成する３Ｄ位置の点を中心として想定位置誤差に相当する半径の円または楕円を考えて、円どうしが重なるか否かを判定するようにしてもよい。
【0137】
あるいは、片方の経路だけを構成する３Ｄ位置の点（または誤差を考慮した円）とし、もう片方の経路は折れ線（または誤差を考慮して幅を持たせた太い線＝線を囲うような形状）とみなして、点（または円）と折れ線（形状）との重なり判定をおこなうようにしてもよい。
【0138】
図１０は、マップの経路情報を形状とした例を示す説明図である。図１０において、重なり比較をする際に、３Ｄ環境マップ５２２の「車載データの経路情報」８３１の走行経路の位置折れ線に、太さを持たせた形状を使う。具体的には、ＧＰＳ誤差１００１を考慮して、太さを持たせることができる。「□」２０２および「□」２０２から延びる矢印１０００によって、「車載データの経路情報」８３１の経路位置および進行方向を示している。
【0139】
位置姿勢推定したい車載データのＧＮＳＳ（ＧＰＳ）経路が、この形状領域内であれば、この３Ｄ環境マップを使うと判断する。さらに、マップの各経路位置での撮影方向と、ＧＮＳＳによる経路方向が合致するかを判定する。たとえば、対向車線のマップなら、逆方向になる。また、図示省略するが、逆に位置姿勢推定したい車載データのＧＮＳＳ（ＧＰＳ）経路を誤差１００１を考慮した太さのある形状とみなし、マップ走行軌跡折れ線と比較してもよい。このように、誤差を考慮して幅を持たせた太い線（線を囲うような所定形状１００２）を用いて重なりを判定するようにしてもよい。
【0140】
このとき、たとえば、単に重なるかどうかだけではなく、経路のどれだけが重なったかも合わせて算出するようにしてもよい。ここで、点（円）どうしの判定の場合には、双方の経路を構成する全点数のうち、他経路の点群と重なったと判定された点群の割合を調べて、重なった割合とすることができる。本実施の形態１では、双方の経路の重なった割合を簡単に算出できる、円どうしの判定を実行する。また、点（円）と折れ線（形状）の判定の場合は、同様に全点数に対して重なった点群の割合を求めて、重なった割合とすることができる。
【0141】
このようにして、位置姿勢推定対象経路において、各３Ｄ環境マップごとに重なった割合を求めると同時に、各３Ｄ環境マップにおいても、位置姿勢推定対象経路と重なった割合を求めることができる。なお、本経路の点や円、折れ線、形状を用いた重なり判定計算方法は一例であり、これらの方法以外の他の方法で重なりを求めてもよい。
【0142】
移動経路・移動方向比較部５１３は、算出した重なった割合を見て、位置姿勢対象経路において重なった割合が無かった３Ｄ環境マップを、利用対象候補の３Ｄ環境マップ群から除外することができる（「ステップＳ１」）。図４では、重なり判定をおこなった結果、１２個の３Ｄ環境マップのうち、５個の３Ｄ環境マップ（３Ｄ環境マップ３０１、３０５、３０６、３１０、３１１）のマップ群は重なりがないことから、算出用３Ｄ環境マップの候補から削除する。
【0143】
つぎに、移動経路・移動方向比較部５１３は、利用対象候補に残った７個の３Ｄ環境マップ（３０２～３０４、３０７～３０９、３１２）のマップ群に対して、進行方向に関する判定、すなわち、重なると判定した経路位置で類似する進行方向であるか否かを判定する（「ステップＳ２」）。
【0144】
具体的には、３Ｄ環境マップの経路情報８０３の進行方向情報と、位置姿勢推定対象経路の進行方向を比較する。たとえば、各経路の各ベクトルどうしの方向を比較し、ベクトルの内積や成す角を求めて、明らかに反対方向だったり、直交したりするような３Ｄ環境マップを除外する。これにより、反対車線または交差道路と判断できる経路に関係する３Ｄ環境マップや、車載データのデータ取得方向が反対方向で見えが大きく異なると思われる車載データから作成した３Ｄ環境マップを除外することができる。なお、この除外方法は一例であり、３Ｄ環境マップが上下車線で１つのマップとして作成できる場合には、方向が正反対に近い、反対車線のマップを除外しなくてもよい。
【0145】
図４では、重なり判定をおこなった結果、１２個の３Ｄ環境マップのうち、４個の３Ｄ環境マップ（３Ｄ環境マップ３０７、３０８、３０９、３１２）のマップ群の進行方向が逆または直交に近いことから、算出用３Ｄ環境マップの候補から削除する。この結果、残りの３個の３Ｄ環境マップ（３０２、３０３、３０４）のマップ群が、移動経路・移動方向比較部５１３で選定される。
【0146】
移動経路・移動方向比較部５１３は、このように、経路の位置と進行方向から経路の類似性を判断し、利用する３Ｄ環境マップを絞り込む。なお、移動経路・移動方向比較部５１３は、さらに、残った３Ｄ環境マップ群内で経路の重なり率が低いマップを除外するようにしてもよい。あるいは、位置姿勢推定部５１５において利用する優先度を、経路の重なり率が低いマップほど、他の３Ｄ環境マップに比べて低い優先度に決定するようにしてもよい。
【0147】
位置姿勢推定対象経路に対する３Ｄ環境マップの重なり率が低い３Ｄ環境マップは、実際に位置姿勢推定部５１５において利用できる部分が少ないと考えられる。したがって、当該３Ｄ環境マップを読み込んで展開して位置姿勢推定処理を実行しても、すぐに位置姿勢が推定できなくなる。このように、位置姿勢推定処理を実行するための準備コスト（展開メモリ量や、読み込み展開処理コストなど）のわりに推定できる経路部分が少ないので、他に３Ｄ環境マップが存在する場合には、そちらの利用を優先した方が、位置姿勢推定処理の効率を上げることができる。また、位置姿勢推定対象の経路全体で位置姿勢推定を完了できる時間を短縮することができる。
【0148】
なお、重なり率が低いマップであっても、マップ取得部５１４が３Ｄ環境マップ５２２の全部（丸ごと）取得するのではなく、重なり部分だけを特定して取得できるのであれば、マップ取得および読み込み展開コストは低くなるため、除外したり、優先度を下げなくてもよい。また、重なり率が低いマップしかなかった場合には、位置姿勢推定処理に用いる３Ｄ環境マップが見つからなかったとして、位置姿勢推定処理を断念するようにしてもよい。
【0149】
移動経路・移動方向比較部５１３は、位置姿勢推定対象経路と３Ｄ環境マップ５２２に関する経路情報８０３との比較だけではなく、従来の探索方法のように、位置姿勢推定対象の車載データの取得年月日や取得時刻、取得車両種類などによっても、３Ｄ環境マップ５２２を選定（選別）することができる。年月日や時刻が近い３Ｄ環境マップほど、位置姿勢推定対象の車載データと類似するシーンで取得した車載データから作成されているので、シーン変化が少ない。そのため、位置姿勢推定部５１５における位置姿勢推定処理が確実に実行できる可能性が高くなる。この選定は、経路位置や進行方向での判定の前後、または判定中のいずれのタイミングで実施してもよい。
【0150】
また、移動経路・移動方向比較部５１３は、経路の位置の判定を実施する際に、３Ｄ環境マップ５２２の高さの判定についておこなうようにしてもよい。高さを考慮しない緯度経度での判定の場合は、たとえば経路の重なり具合の判定として、２次元の点、または、円、折れ線、形状による判定などをおこなうことができる。一方で、高さを含んだ判定をおこなう場合は、たとえば経路の重なり具合の判定として、３次元の点、または球、３次元折れ線、３次元形状による判定などをおこなうことができる。
【0151】
また、２次元での判定を実施した後で、位置姿勢推定対象経路の高さから求めた任意の高さ閾値、たとえば高さの最小値または最大値と、各３Ｄ環境マップ５２２を比較し、それらの高さ閾値に合致した３Ｄ環境マップ５２２の経路（たとえば、高さの最小値以上、かつ、最大値以下の高さの経路位置しかなかった経路）のみを、経路位置が類似するもの（重なるもの）と判断するようにしてもよい。
【0152】
車載データと同時取得したＧＮＳＳ情報に高さ情報が含まれる場合には、３Ｄ環境マップ５２２や、位置姿勢推定対象経路の高さに標高値が含まれており、高さの判定をおこなうことで、高架道路の上下道を区別した３Ｄ環境マップ５２２の選定をおこなうことができる。
【0153】
さらに、移動経路・移動方向比較部５１３は、上述した経路の位置および進行方向による３Ｄ環境マップ５２２の選定処理をおこなう前に、従来と同様の地域メッシュによる選定をおこなうようにしてもよい。事前に明らかに地域が異なる地域メッシュの３Ｄ環境マップを排除することで、不要な３Ｄ環境マップとの重なり具合の計算などを省略することができる。
【0154】
マップ登録部５１２において前述したように、移動経路・移動方向比較部５１３での本選定を高速化するために、一般的な空間データベースの空間クエリ高速化と同様に生成した、３Ｄ環境マップＤＢ５１０のインデックス情報／空間インデックス情報を利用して、これら３Ｄ環境マップ５２２の特定をおこなうようにしてもよい。
【0155】
なお、移動経路・移動方向比較部５１３での本選定を高速化するために、一般的なデータベースのクエリ高速化と同様に、３Ｄ環境マップＤＢ５１０のデータ構造化要素８０２に関するインデックスを利用してもよい。一般的に、ＤＢ内で検索にかかわる要素を用いて検索専用の別テーブルを作成することで検索速度向上が図れることから、ＤＢの一般的な既知のインデックス情報の作成手段を用いて、車載データの取得位置を用いた検索用のインデックス情報をあらかじめ準備しておくようにしてもよい。インデックスは、たとえば、上述したように、マップ登録部５１２により作成することができる。これにより、移動経路・移動方向比較部５１３における比較処理をより高速化することができる。
【0156】
（マップ取得部５１４の内容）
マップ取得部５１４は、移動経路・移動方向比較部５１３において選定された３Ｄ環境マップ５２２群に対して、実際に３Ｄ環境マップＤＢ５１０からマップデータを特定し、取得する。なお、移動経路・移動方向比較部５１３がマップデータの特定までおこない、マップ取得部５１４は、特定されたマップデータを取得するだけであってもよい。すなわち、マップデータの特定は、移動経路・移動方向比較部５１３、マップ取得部５１４のどちらかがおこなってもよい。
マップ取得部５１４は、移動経路・移動方向比較部５１３において選定された３Ｄ環境マップ５２２群に対して、実際に３Ｄ環境マップＤＢ５１０からマップデータを特定し、取得する。なお、移動経路・移動方向比較部５１３がマップデータの特定までおこない、マップ取得部５１４は、特定されたマップデータを取得するだけであってもよい。すなわち、マップデータの特定は、移動経路・移動方向比較部５１３、マップ取得部５１４のどちらかがおこなってもよい。
【0157】
従来方法と異なり、緯度経度だけで選定した３Ｄ環境マップではなく、移動経路・移動方向比較部５１３の画像特徴比較処理を通して、位置姿勢推定対象の経路と同じ道路を同じ上り下り方向で走行した車載データで作成した３Ｄ環境マップ５２２に、データ取得対象を限定しているため、従来方法よりも低コストで３Ｄ環境マップ５２２を取得することができる。
【0158】
また、３Ｄ環境マップ５２２を複数取得する場合などでは、移動経路・移動方向比較部５１３において算出した、重なり具合などから決定した優先度や、位置姿勢推定対象の経路のどこと重なるマップであるかを、取得したマップデータとともに保持するようにしてもよい。そして、位置姿勢推定部５１５においては、取得した優先度に基づいて、または、重なり具合が経路全体を網羅できるようなマップ群を使って算出することにより、より精度の高い位置姿勢の推定処理をおこなうことができる。
【0159】
マップ取得部５１４は、具体的には、たとえば、図６に示したサーバ５０１において、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、あるいは、ネットワークＩ／Ｆ６０３、記録媒体Ｉ／Ｆ６０４などによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した車載機５０２において、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、あるいは、無線通信装置７０３などによって、その機能を実現するようにしてもよい。
【0160】
（位置姿勢推定部５１５の内容）
位置姿勢推定部５１５は、マップ取得部５１４によって取得された３Ｄ環境マップ５２２のデータと、位置姿勢推定対象である「車載データ＋ＧＮＳＳ情報」５２１のデータを用いて、従来の方法、たとえばＳＬＡＭなどによって、車載データの取得センサ（車載機５０２）および移動体５０３の位置姿勢を推定する。具体的には、選定取得した３Ｄ環境マップ５２２のデータを読み込んでメモリ展開し、同様に読み込んでメモリ展開した車載データと比較して、ＳＬＡＭなどによる計算方法を用いて位置姿勢を推定することができる。
位置姿勢推定部５１５は、マップ取得部５１４によって取得された３Ｄ環境マップ５２２のデータと、位置姿勢推定対象である「車載データ＋ＧＮＳＳ情報」５２１のデータを用いて、従来の方法、たとえばＳＬＡＭなどによって、車載データの取得センサ（車載機５０２）および移動体５０３の位置姿勢を推定する。具体的には、選定取得した３Ｄ環境マップ５２２のデータを読み込んでメモリ展開し、同様に読み込んでメモリ展開した車載データと比較して、ＳＬＡＭなどによる計算方法を用いて位置姿勢を推定することができる。
【0161】
選定したマップが複数ある場合に、どれを使うかを検討する材料が無かった従来方法と異なり、移動経路・移動方向比較部５１３で重なり具合などから決定した優先度を持つため、優先度が高い順に位置姿勢推定処理をおこなうことで、一度の位置姿勢推定処理の実行で位置姿勢推定対象の位置姿勢推定を完了できる可能性が増加する。また、できるだけ少ないマップで、位置姿勢推定対象経路のすべての３Ｄ位置に対して重なりを網羅するように、マップをさらに選定して位置姿勢推定処理をおこない、位置姿勢推定が失敗したときだけ残りのマップを使うようにすることで、最小限のマップによる、低処理コストな位置姿勢推定（算出）を実現することができる。
【0162】
また、複数マップを優先順に位置姿勢推定（算出）して、位置姿勢推定が成功した場合に、他のマップによる位置姿勢推定をおこなわないようにしてもよい。一方、複数マップすべての位置姿勢推定結果を算出して、それらを比較して最終的な位置姿勢推定結果とするようにしてもよい。
【0163】
たとえば、各マップの位置姿勢推定結果を比較し、そのうちの１つのマップ、たとえば位置姿勢推定できた車載データのデータ数（時刻数）が最も多いマップの結果を最終的な結果としてもよい。また、複数マップでの各車載データ（各時刻）の位置姿勢推定結果を統計処理、たとえば平均化して最終的な結果とすることで、より高精度で確実に位置姿勢推定をおこなうようにしてもよい。このとき、優先度を重みとして用いて、より優先度の高いマップの結果を重視反映させた統計処理としてもよい。
【0164】
選定した全３Ｄ環境マップ５２２群のループ処理として、マップ取得部５１４の処理と位置姿勢推定部５１５の処理をマップ１つずつに対して纏めて実行するようにしてもよい。その場合には、３Ｄ環境マップ５２２のマップ取得コストもあわせて低コストに抑えることができる。
【0165】
位置姿勢推定部５１５は、具体的には、たとえば、図６に示したサーバ５０１において、メモリ６０２に記憶されたプログラムをＣＰＵ６０１が実行することによって、その機能を実現することができる。また、具体的には、たとえば、図７に示した車載機５０２において、メモリ７０２に記憶されたプログラムをＣＰＵ７０１が実行することによって、その機能を実現するようにしてもよい。
【0166】
（移動経路・移動方向比較部５１３、マップ取得部５１４、位置姿勢推定部５１５の処理手順）
図１１は、実施の形態１にかかる移動経路・移動方向比較部、マップ取得部および位置姿勢推定部の処理の一例を示すフローチャートである。図１１のフローチャートにおいて、移動経路比較処理、すなわち、３Ｄ環境マップＤＢ５１０において、各マップの車載データの経路情報と、位置姿勢推定対象の走行経路であるＧＮＳＳ情報（ＧＰＳ）を比較し、マップ経路情報と走行経路の重複率を計算し、重複率が閾値以上の３Ｄ環境マップ群を選定する（ステップＳ１１０１）。
図１１は、実施の形態１にかかる移動経路・移動方向比較部、マップ取得部および位置姿勢推定部の処理の一例を示すフローチャートである。図１１のフローチャートにおいて、移動経路比較処理、すなわち、３Ｄ環境マップＤＢ５１０において、各マップの車載データの経路情報と、位置姿勢推定対象の走行経路であるＧＮＳＳ情報（ＧＰＳ）を比較し、マップ経路情報と走行経路の重複率を計算し、重複率が閾値以上の３Ｄ環境マップ群を選定する（ステップＳ１１０１）。
【0167】
そして、３Ｄ環境マップは選定できて、選定された３Ｄ環境マップがあるか否かを判断する（ステップＳ１１０２）。ここで、３Ｄ環境マップが選定できずなかったか、または、選定された３Ｄ環境マップがない場合（ステップＳ１１０２：Ｎｏ）は、何もせずに、一連の処理を終了する。
【0168】
一方、３Ｄ環境マップは選定でき、選定された３Ｄ環境マップがある場合（ステップＳ１１０２：Ｙｅｓ）は、つぎに、位置姿勢推定対象の車載データの車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知であり、車載データの走行経路の進行方向を加工する必要があるか否かを判断する（ステップＳ１１０３）。
【0169】
ここで、位置姿勢推定対象の車載データの車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知であり、車載データの走行経路の進行方向を加工する必要がある場合（ステップＳ１１０３：Ｙｅｓ）は、車載データの走行経路と、車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかを用いて、各経路位置の進行方向（Ａ）を算出する（ステップＳ１１０４）。その後、ステップＳ１１０６へ移行する。
【0170】
一方、位置姿勢推定対象の車載データの車載データセンサ設置位置・データ取得方向・車載データ種類のいずれかが既知ではないか、車載データの走行経路の進行方向を加工する必要がない場合（ステップＳ１１０３：Ｎｏ）は、車載データの走行経路から、各経路位置の進行方向（Ａ）を算出する（ステップＳ１１０５）。その後、ステップＳ１１０６へ移行する。
【0171】
つぎに、各選定マップにて、経路情報の走行位置と進行方向（Ｂ）を取得する（ステップＳ１１０６）。そして、移動方向比較処理、すなわち、各選定マップにて、推定対象データの各経路位置に最も近い経路位置を探し、その位置での進行方向（Ｂ）と、推定対象データの該当経路位置での進行方向（Ａ）を比較し、当該マップの比較できた総数に対して、方向類似度が低かった（異なる方向だった）比較結果の割合が閾値以上なら、利用対象マップから外す（ステップＳ１１０７）。その後、ステップＳ１１０８へ移行する。ここまでの処理（ステップＳ１１０１～Ｓ１１０７の各処理）は、たとえば、移動経路・移動方向比較部５１３によっておこなうことができる。
【0172】
つぎに、利用対象マップが残った（利用対象マップはある）か否かを判断する（ステップＳ１１０８）。ここで、利用対象マップが残らなかった場合（ステップＳ１１０８：Ｎｏ）は、一連の処理を終了する。一方、利用対象マップが残った場合（ステップＳ１１０８：Ｙｅｓ）は、残った利用対象マップの情報（３Ｄマップ要素８０１など）を３Ｄ環境マップＤＢ５１０から取得する（ステップＳ１１０９）。その後、ステップＳ１１１０へ移行する。これらの処理（ステップＳ１１０８、Ｓ１１０９の各処理）は、たとえば、マップ取得部５１４によっておこなうことができる。
【0173】
その後、取得した３Ｄ環境マップ５２２のＤＢ情報（３Ｄマップ要素８０１など）と対象車載データとＧＮＳＳ情報とを用いて、推定対象画像の位置姿勢推定処理を実行する（ステップＳ１１１０）。このステップＳ１１１０の処理は、たとえば、位置姿勢推定部５１５によっておこなうことができる。
【0174】
以上説明したように、実施の形態１によれば、道路ネットワーク情報などの他の情報を用いることなく、位置姿勢推定したい車載データに関する３Ｄ環境マップを、より同じ道路を同じ方向で走行した車載データで作成した３Ｄ環境マップ５２２を優先する優先度付きで特定することができる。そのため、他のデータベースなどの作成管理コストを必要とせず、無駄な３Ｄ環境マップのデータ取得コストおよび位置姿勢推定処理の実行コストを削減し、位置姿勢推定完了までの時間を短縮し、位置姿勢推定をリアルタイムで実施することが可能となる。
【0175】
他のデータベースの作成管理などを必要とせずに、地域メッシュごとのマップではなく、位置姿勢推定したい車載データの走行経路に似た経路を走行方向に似た方向で走行した車載データで作成したマップだけを、ピンポイントで探索することができる。
【0176】
したがって、従来は、同じ地域メッシュ内の隣道路／交差路／路地／対向車線道路などの関係ない道路も含めた多数の３Ｄ環境マップ群の中で、どの３Ｄ環境マップが正しいかわからず、「多数の３Ｄ環境マップ群から一つを選択しては、選択された３Ｄ環境マップを使用して、対象画像を撮影した車両の位置姿勢推定を実行する」という処理を、位置姿勢推定ができるまで何度も繰り返していたが、実施の形態１にかかる位置姿勢算出方法を用いることで、正しい３Ｄ環境マップをピンポイントで探索でき、より高速に位置姿勢推定結果を得ることができる。
【0177】
（実施の形態２）
つぎに、実施の形態２について説明する。実施の形態１においては、位置姿勢推定部５１５は、位置姿勢推定処理をおこなうごとに、選定取得した３Ｄ環境マップ５２２を読み込んでメモリ展開して、ＳＬＡＭなどによる計算方法で位置姿勢推定処理をおこなうという構成である。これに対して、実施の形態２では、図１１に示すように、あらかじめメモリ展開済のマップ展開の３Ｄ環境マップ５２２を保管したマップ展開・位置姿勢推定サーバ１２０２（１２０２ａ、１２０２ｂ、１２０２ｃ、・・・）を用意することを特徴とする。
つぎに、実施の形態２について説明する。実施の形態１においては、位置姿勢推定部５１５は、位置姿勢推定処理をおこなうごとに、選定取得した３Ｄ環境マップ５２２を読み込んでメモリ展開して、ＳＬＡＭなどによる計算方法で位置姿勢推定処理をおこなうという構成である。これに対して、実施の形態２では、図１１に示すように、あらかじめメモリ展開済のマップ展開の３Ｄ環境マップ５２２を保管したマップ展開・位置姿勢推定サーバ１２０２（１２０２ａ、１２０２ｂ、１２０２ｃ、・・・）を用意することを特徴とする。
【0178】
そもそも３Ｄ環境マップはそのデータ量が多いため、位置姿勢推定処理をおこなうたびに、３Ｄ環境マップを読み込み展開していたのでは、リアルタイムな位置姿勢推定処理を実行することが難しい。このため実施の形態２では、位置姿勢推定対象の車載データとＧＮＳＳ情報が入力されるごとに、３Ｄ環境マップを特定して読み込み展開するのではなく、あらかじめ３Ｄ環境マップを展開するマップ展開・位置姿勢推定サーバ１２０２を用意する。
【0179】
そして、３Ｄ環境マップを特定する際に、あらかじめ該当する３Ｄマップを展開しているマップ展開・位置姿勢推定サーバ１２０２を特定して、当該マップ展開・位置姿勢推定サーバ１２０２を利用し、位置姿勢推定処理に必要な情報を取得する。これにより、３Ｄ環境マップを取得し、取得した後に当該３Ｄ環境マップを自サーバにおいて読み込み展開をするという処理をおこなうことなく、位置姿勢推定処理を実行することができる。このようにして、リアルタイムな位置姿勢推定を実現することができる。
【0180】
図１２は、実施の形態２にかかる位置姿勢算出方法を実現するシステム構成の一例を示す説明図である。また、図１３は、実施の形態２にかかる３Ｄ環境マップの構成の一例を示す説明図である。図１２において、実施の形態２にかかる位置姿勢算出方法を実現する移動体位置姿勢算出システム１２００は、サーバ１２０１と、移動体５０３に搭載される車載機５０２とを備える。
【0181】
なお、図１１に示す実施の形態２にかかる位置姿勢算出方法を実現する移動体位置姿勢算出システム１２００において、図５に示した実施の形態１にかかる位置姿勢算出方法を実現する移動体位置姿勢算出システム５００と同じ構成部については同一の符号を付して、その詳細な説明を省略する。同様に、図１３に示す実施の形態２にかかる３Ｄ環境マップ１２２２において、図８に示した３Ｄ環境マップ５２２と同じ構成部については同一の符号を付して、その詳細な説明を省略する。
【0182】
マップ展開登録部１２１２は、環境マップ作成部５１１で作成した３Ｄ環境マップ５２２を、３Ｄ環境マップＤＢ５１０に登録するとともに、任意の処理サーバ（マップ展開・位置姿勢推定サーバ１２０２（１２０２ａ～１２０２ｃ））に読み込み展開させ、当該マップ展開・位置姿勢推定サーバ１２０２の情報（図１３に示す展開サーバ情報１３０１）を３Ｄ環境マップＤＢ５１０に保持させる。
【0183】
図１３において、３Ｄ環境マップ１２２２（１２２２ａ～１２２２ｃ）は、図８に示した３Ｄ環境マップ５２２（５２２ａ～５２２ｃ）と同様に、３Ｄマップ要素（３Ｄ画像特徴）８０１、データ構造化要素８０２、経路情報８０３の各要素を保持する。３Ｄ環境マップ１２２２のこれらの各要素の内容も３Ｄ環境マップ５２２の各要素の内容と同じものである。３Ｄ環境マップ１２２２（１２２２ａ～１２２２ｃ）では、３Ｄマップ要素８０１、データ構造化要素８０２、経路情報８０３の各要素に加えて、展開サーバ情報１３０１を含んでいる。
【0184】
このようにして、複数の３Ｄ環境マップを、当該環境マップの任意の画像情報の撮影装置の位置姿勢の算出をおこなう他の情報処理装置であるマップ展開・位置姿勢推定サーバ１２０２にあらかじめメモリ展開しておくことができる。また、３Ｄ環境マップ１２２２に、当該環境マップがメモリ展開されている情報処理装置（マップ展開・位置姿勢推定サーバ１２０２）に関する「展開サーバ情報１３０１」を登録することができる。
【0185】
なお、展開サーバ情報１３０１は、環境マップ１２２２の一部ではなく、環境マップ１２２２から参照できる別の情報およびＤＢに保持してもよい。すなわち、展開サーバ情報１３０１は、データを実装によっては、別データおよびＤＢに分割保持していてもよい。このように、展開サーバ情報１３０１は、当該環境マップがメモリ展開されている情報処理装置（マップ展開・位置姿勢推定サーバ１２０２）が容易に特定できるように保持されているとよい。
【0186】
マップ展開・位置姿勢推定サーバ１２０２（１２０２ａ～１２０２ｃ）は、ＳＬＡＭなどの位置姿勢推定処理をおこなう処理サーバである。マップ展開登録部１２１２において指定された任意の３Ｄ環境マップ５２２をあらかじめ読み込んでメモリ展開しておき、ストリームや任意の通信手段によって任意タイミングで処理したい車載データおよびＧＮＳＳ情報を取得して、位置姿勢推定を計算して出力する。
【0187】
この結果、よりリアルタイムな位置姿勢推定処理が可能となる。１つのマップ展開・位置姿勢推定サーバ１２０２において展開する３Ｄ環境マップ５２２の数は任意であるが、３Ｄ環境マップ５２２を複数、展開した方が、一度に展開した３Ｄ環境マップ５２２群すべてを使った位置姿勢推定ができる。したがって、特に複数のマップからなる経路を走行した車載データの位置姿勢推定をおこなう場合などに、処理時間をより短くできるという利点がある。
【0188】
移動経路・移動方向比較部１２１３は、実施の形態１における移動経路・移動方向比較部５１３と同様に、移動体５０３または当該移動体５０３に搭載された取得装置の一例である車載機５０２がデータを取得する際の取得位置を含む複数の３Ｄ環境マップ５２２の経路情報（図８に示した経路情報８０３）と、位置姿勢の算出の対象となる対象移動体５０３または当該対象移動体に搭載された取得装置（車載機５０２）から取得した経路情報との比較をおこなう。
【0189】
そして、その比較結果に基づいて、３Ｄ環境マップ１２２２を特定する。さらに、特定した３Ｄ環境マップ１２２２に含まれるマップ展開・位置姿勢推定サーバ１２０２に関する情報である展開サーバ情報１３０１から、特定した３Ｄ環境マップ１２２２を展開しているマップ展開・位置姿勢推定サーバ１２０２を特定する。
【0190】
このようにして、複数の環境マップを、当該環境マップの任意の車載データの取得位置姿勢の算出をおこなう他の情報処理装置であるマップ展開・位置姿勢推定サーバ１２０２にあらかじめメモリ展開しておき、特定したマップ展開・位置姿勢推定サーバ１２０２に関する情報である展開サーバ情報１３０１を含めて、３Ｄ環境マップ１２２２（１２２２ａ～１２２２ｃ）とすることで、特定された算出用環境マップがメモリ展開されているマップ展開・位置姿勢推定サーバ１２０２を特定することができる。
【0191】
位置姿勢推定部１２１４は、特定されたマップ展開・位置姿勢推定サーバ１２０２に対して、位置姿勢推定対象の車載データおよびＧＮＳＳ情報を投入して位置姿勢を算出し、推定位置姿勢５２３を出力する。このようにして、特定されたマップ展開・位置姿勢推定サーバ１２０２によって算出された位置姿勢結果を取得することができる。
【0192】
（環境マップ作成部５１１・マップ展開登録部１２１２の処理手順）
図１４は、実施の形態２にかかる環境マップ作成部およびマップ展開登録部の処理の一例を示すフローチャートである。図１４のフローチャートにおいて、ステップＳ１４０１～ステップＳ１４０５の各処理は、図９のフローチャートにおけるステップＳ９０１～ステップＳ９０５の各処理と同じ内容である。すなわち、ステップＳ１４０１～ステップＳ１４０５の処理は、環境マップ作成部５１１によっておこなうことができる。
図１４は、実施の形態２にかかる環境マップ作成部およびマップ展開登録部の処理の一例を示すフローチャートである。図１４のフローチャートにおいて、ステップＳ１４０１～ステップＳ１４０５の各処理は、図９のフローチャートにおけるステップＳ９０１～ステップＳ９０５の各処理と同じ内容である。すなわち、ステップＳ１４０１～ステップＳ１４０５の処理は、環境マップ作成部５１１によっておこなうことができる。
【0193】
ステップＳ１４０５に引き続き、３Ｄ環境マップ５２２（３Ｄ画像特徴８０１）をマップ展開・位置姿勢推定サーバ１２０２に展開する（ステップＳ１４０６）。そして、経路情報８０３、展開サーバ情報１３０１を含む３Ｄ環境マップ１２２２を、３Ｄ環境マップＤＢ５１０に登録する（ステップＳ１４０７）。これにより、一連の処理を終了する。このステップＳ１４０６、Ｓ１４０７の各処理は、たとえば、マップ展開登録部１２１２によっておこなうことができる。
【0194】
（移動経路・移動方向比較部１２１３、位置姿勢推定部１２１４の処理手順）
図１５は、実施の形態２にかかる移動経路・移動方向比較部および位置姿勢推定部の処理の一例を示すフローチャートである。図１５のフローチャートにおいて、ステップＳ１５０１～ステップＳ１５０８の各処理は、図１１のフローチャートにおけるステップＳ１１０１～ステップＳ１１０８の各処理と同じ内容である。すなわち、ステップＳ１５０１～ステップＳ１５０８の処理は、移動経路・移動方向比較部１２１３によっておこなうことができる。
図１５は、実施の形態２にかかる移動経路・移動方向比較部および位置姿勢推定部の処理の一例を示すフローチャートである。図１５のフローチャートにおいて、ステップＳ１５０１～ステップＳ１５０８の各処理は、図１１のフローチャートにおけるステップＳ１１０１～ステップＳ１１０８の各処理と同じ内容である。すなわち、ステップＳ１５０１～ステップＳ１５０８の処理は、移動経路・移動方向比較部１２１３によっておこなうことができる。
【0195】
ステップＳ１５０８に引き続き、残った利用対象マップの展開サーバ情報１３０１を３Ｄ環境マップＤＢ５１０から取得する（ステップＳ１５０９）。つぎに、対象車載データ・ＧＮＳＳ情報を、取得した展開サーバ情報１３０１によって特定したマップ展開・位置姿勢推定サーバ１２０２に入力する（ステップＳ１５１０）。そして、マップ展開・位置姿勢推定サーバ１２０２から位置姿勢推定結果を取得する（ステップＳ１５１１）。これにより、一連の処理を終了する。このステップＳ１５０９～Ｓ１５１１の各処理は、たとえば、位置姿勢推定部１２１４によっておこなうことができる。
【0196】
なお、実施の形態１と実施の形態２を混合したシステムとすることもできる。すなわち、位置姿勢推定の要望が多い、または、よりリアルタイムな位置姿勢推定の要望のある道路に関する３Ｄ環境マップなどの一部の３Ｄ環境マップだけを、実施の形態２に示すように、マップ展開・位置姿勢推定サーバ１２０２に展開して、位置姿勢推定処理を実行する。一方、それら以外の道路、位置姿勢推定の要望が少ない、または、よりリアルタイムな位置姿勢推定の要望がない道路では、実施の形態１に示すように、位置姿勢推定の要望がくるたびに随時マップを展開して位置姿勢推定処理を実施するようにしてもよい。
【0197】
たとえば、多数の車両が走行する幹線道路などの重要な道路だけ、マップ展開・位置姿勢推定サーバ１２０２に展開することで、サーバコストをできるだけ抑えつつ、かつ、できるだけ多くの位置姿勢推定の要求をリアルタイムに実現するシステムとすることができる。
【0198】
以上説明したように、実施の形態２によれば、あらかじめマップ展開登録部１２１２において、マップ展開・位置姿勢推定サーバ１２０２にマップの読み込み展開を実施しておく。そして、位置姿勢推定処理をおこなうタイミングでは、マップの読み込み展開を不要とする。これにより、ＳＬＡＭなどにおいて処理のボトルネックとなる読み込み展開時間を短縮（削減）することができる。したがって、より高速かつリアルタイムな位置姿勢推定を実現することができるようになる。
【0199】
なお、本実施の形態で説明した位置姿勢算出方法は、あらかじめ用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することにより実現することができる。プログラム配信プログラムは、ハードディスク、フレキシブルディスク、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｃ）－ＲＯＭ、ＭＯ（Ｍａｇｎｅｔｏ－Ｏｐｔｉｃａｌ Ｄｉｓｋ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリなどのコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また、位置姿勢算出プログラムは、インターネットなどのネットワークを介して配布してもよい。
【0200】
上述した実施の形態に関し、さらに以下の付記を開示する。
【0201】
（付記１）情報処理装置が、
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する、
ことを特徴とする位置姿勢算出方法。
移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する、
ことを特徴とする位置姿勢算出方法。
【0202】
（付記２）前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動経路および移動方向の少なくともいずれかが類似する経路情報を有する環境マップである、
ことを特徴とする付記１に記載の位置姿勢算出方法。
ことを特徴とする付記１に記載の位置姿勢算出方法。
【0203】
（付記３）前記算出用環境マップは、前記対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報のうちの移動経路および移動方向が類似する経路情報を有する環境マップである、
ことを特徴とする付記１に記載の位置姿勢算出方法。
ことを特徴とする付記１に記載の位置姿勢算出方法。
【0204】
（付記４）移動経路および移動方向は、前記取得装置の種類、前記取得装置の設置位置、前記取得装置が取得するデータの取得方向の少なくともいずれか一つに基づいて補正する、
ことを特徴とする付記２または３に記載の位置姿勢算出方法。
ことを特徴とする付記２または３に記載の位置姿勢算出方法。
【0205】
（付記５）前記環境マップの経路情報の各位置、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得したデータの各取得位置を中心とした任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいて、前記算出用環境マップを特定する、
ことを特徴とする付記１～４のいずれか一つに記載の位置姿勢算出方法。
ことを特徴とする付記１～４のいずれか一つに記載の位置姿勢算出方法。
【0206】
（付記６）前記環境マップの経路情報、または、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報における経路を含む任意の大きさの２次元または３次元の所定形状を設定し、当該所定形状の形状内外判定を用いて、経路の重なり度を判定し、当該判定の結果に基づいて、前記算出用環境マップを特定する、
ことを特徴とする付記１～４のいずれか一つに記載の位置姿勢算出方法。
ことを特徴とする付記１～４のいずれか一つに記載の位置姿勢算出方法。
【0207】
（付記７）前記情報処理装置が、
前記経路の重なり度に基づいて、特定された前記算出用環境マップの利用優先度を算出する、
ことを特徴とする付記５または６に記載の位置姿勢算出方法。
前記経路の重なり度に基づいて、特定された前記算出用環境マップの利用優先度を算出する、
ことを特徴とする付記５または６に記載の位置姿勢算出方法。
【0208】
（付記８）前記情報処理装置が、
前記複数の環境マップを、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出をおこなう他の情報処理装置にあらかじめメモリ展開しておき、
特定された前記算出用環境マップがメモリ展開されている情報処理装置を特定する、
ことを特徴とする付記１～７のいずれか一つに記載の位置姿勢算出方法。
前記複数の環境マップを、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出をおこなう他の情報処理装置にあらかじめメモリ展開しておき、
特定された前記算出用環境マップがメモリ展開されている情報処理装置を特定する、
ことを特徴とする付記１～７のいずれか一つに記載の位置姿勢算出方法。
【0209】
（付記９）前記情報処理装置が、
前記環境マップに、当該環境マップがメモリ展開されている情報処理装置に関する情報を登録する、
ことを特徴とする付記８に記載の位置姿勢算出方法。
前記環境マップに、当該環境マップがメモリ展開されている情報処理装置に関する情報を登録する、
ことを特徴とする付記８に記載の位置姿勢算出方法。
【0210】
（付記１０）前記情報処理装置が、
特定された情報処理装置によって算出された位置姿勢結果を取得する、
ことを特徴とする付記８または９に記載の位置姿勢算出方法。
特定された情報処理装置によって算出された位置姿勢結果を取得する、
ことを特徴とする付記８または９に記載の位置姿勢算出方法。
【0211】
（付記１１）前記環境マップの前記経路情報に対して、検索用のインデックスを作成し、
前記インデックスを用いて、複数の環境マップを探索することを特徴とする付記１～８のいずれか一つに記載の位置姿勢算出方法。
前記インデックスを用いて、複数の環境マップを探索することを特徴とする付記１～８のいずれか一つに記載の位置姿勢算出方法。
【0212】
（付記１２）移動体または当該移動体に搭載された取得装置がデータを取得する際の取得位置を含む複数の環境マップの経路情報と、位置姿勢の算出の対象となる対象移動体または当該対象移動体に搭載された取得装置から取得した経路情報との比較をおこない、
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させる、
ことを特徴とする位置姿勢算出プログラム。
前記比較の結果に基づいて、前記複数の環境マップの中から、前記対象移動体または当該対象移動体に搭載された取得装置がデータを取得する際の位置姿勢の算出に用いる算出用環境マップを特定する処理を情報処理装置に実行させる、
ことを特徴とする位置姿勢算出プログラム。
【符号の説明】
【0213】
１０１、１０２、１０３ 道路
２０１、８０１ ３Ｄマップ要素（３Ｄ画像特徴要素）
２０２、８０２ データ構造化要素
３０１～３１２、８３１ 車載データの経路情報
４００ 車載データ走行経路
５００、１２００ 移動体位置姿勢算出システム
５０１、１２０１ サーバ
５０２ 車載機
５０３ 移動体
５０４ ネットワーク
５０５ 衛星
５１０ ３Ｄ環境マップＤＢ
５１１ 環境マップ作成部
５１２ マップ登録部
５１３、１２１３ 移動経路・移動方向比較部
５１４ マップ取得部
５１５、１２１４ 位置姿勢推定部
５２１ 車載データ＋ＧＮＳＳ情報
５２２（５２２ａ～５２２ｃ）、１２２２（１２２２ａ～１２２２ｃ） ３Ｄ環境マップ
５２３ 推定位置姿勢
８０３ 経路情報
８１１ ３次元実座標位置
８１２ 特定情報（特徴要素を特定するための情報）
８２１ 関係する３Ｄマップ要素群
１２０２（１２０２ａ～１２０２ｃ） マップ展開・位置姿勢推定サーバ
１２１２ マップ展開登録部
１３０１ 展開サーバ情報
２０１、８０１ ３Ｄマップ要素（３Ｄ画像特徴要素）
２０２、８０２ データ構造化要素
３０１～３１２、８３１ 車載データの経路情報
４００ 車載データ走行経路
５００、１２００ 移動体位置姿勢算出システム
５０１、１２０１ サーバ
５０２ 車載機
５０３ 移動体
５０４ ネットワーク
５０５ 衛星
５１０ ３Ｄ環境マップＤＢ
５１１ 環境マップ作成部
５１２ マップ登録部
５１３、１２１３ 移動経路・移動方向比較部
５１４ マップ取得部
５１５、１２１４ 位置姿勢推定部
５２１ 車載データ＋ＧＮＳＳ情報
５２２（５２２ａ～５２２ｃ）、１２２２（１２２２ａ～１２２２ｃ） ３Ｄ環境マップ
５２３ 推定位置姿勢
８０３ 経路情報
８１１ ３次元実座標位置
８１２ 特定情報（特徴要素を特定するための情報）
８２１ 関係する３Ｄマップ要素群
１２０２（１２０２ａ～１２０２ｃ） マップ展開・位置姿勢推定サーバ
１２１２ マップ展開登録部
１３０１ 展開サーバ情報
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】