ホーム > 特許ランキング > 国立大学法人金沢大学
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-03-05
(45)【発行日】2024-03-13
(54)【発明の名称】３次元マップ生成方法及び３次元マップ生成装置
(51)【国際特許分類】
G09B 29/00 20060101AFI20240306BHJP
G06T 7/00 20170101ALI20240306BHJP
【ＦＩ】
G09B29/00 Z
G06T7/00 650A
【請求項の数】
7
(21)【出願番号】P 2020090099
(22)【出願日】2020-05-22
(65)【公開番号】P2021184069
(43)【公開日】2021-12-02
【審査請求日】2023-04-27
【新規性喪失の例外の表示】特許法第３０条第２項適用 令和１年５月２３日に、Ｔｈｅ ３０ｔｈ ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｖｅｈｉｃｌｅｓ Ｓｙｍｐｏｓｉｕｍのウェブサイト（ｈｔｔｐｓ：／／ｉｖ２０１９．ｏｒｇ／ｐｒｏｇｒａｍ／）にて講演要旨が掲載 令和１年６月８日に、Ｔｈｅ ３０ｔｈ ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｖｅｈｉｃｌｅｓ Ｓｙｍｐｏｓｉｕｍにて配布による公開 令和１年６月１０日に、Ｔｈｅ ３０ｔｈ ＩＥＥＥ Ｉｎｔｅｌｌｉｇｅｎｔ Ｖｅｈｉｃｌｅｓ Ｓｙｍｐｏｓｉｕｍにて発表
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２９年度、戦略的情報通信研究開発推進事業、産業技術力強化法第１７条の適用を受ける特許出願
(73)【特許権者】
【識別番号】504160781
【氏名又は名称】国立大学法人金沢大学
(74)【代理人】
【識別番号】100109210
【弁理士】
【氏名又は名称】新居 広守
(72)【発明者】
【氏名】ムハンマド・アムル・アッディバージャ
(72)【発明者】
【氏名】菅沼 直樹
【審査官】鈴木 崇雅
(56)【参考文献】
【文献】特表２０１９－５１２８０２（ＪＰ，Ａ）
【文献】特開２０１９－２１１４６６（ＪＰ，Ａ）
【文献】特開２０１９－１２４８０８（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０９Ｂ ２９／００－１０
Ｇ０６Ｔ ７／００－１９４
(57)【特許請求の範囲】
【請求項1】
車両に搭載され、道路を含む環境を検出するＬｉＤＡＲ（Light Detection and Ranging）から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び前記強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成ステップと、生成された前記３次元データにおける位置誤差を削減することで、３次元マップを生成するマップ生成ステップとを含み、
前記マップ生成ステップは、
前記ＸＹ面マップに対してグラフＳＬＡＭ（Simultaneous Localization and Mapping）を実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減ステップと、
前記ＸＹ面誤差削減ステップによって位置誤差が削減された前記ＸＹ面マップに対応する前記Ｚ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減ステップとを含む
３次元マップ生成方法。
【請求項2】
前記３次元データ生成ステップは、所定の矩形領域における前記強度画像を前記ＸＹ面マップとし、前記強度画像に対応する前記標高画像を前記Ｚ面マップとし、前記ＸＹ面マップと前記Ｚ面マップとの組をノードとする複数のノードの連結を前記３次元データとして生成し、前記複数のノードのそれぞれに対して、対応する前記ＸＹ面マップが示す矩形領域の特定のコーナーのグローバルポジションを識別子として付与する
請求項１記載の３次元マップ生成方法。
【請求項3】
前記３次元データ生成ステップは、前記複数のノードのそれぞれについて、前記車両に搭載されたＧＰＳ（Global Positioning System）から初期位置を取得し、取得された前記初期位置を基準として前記フレームを蓄積していく推測航法により、当該ノードの前記ＸＹ面マップ及び前記Ｚ面マップを生成する請求項２記載の３次元マップ生成方法。
【請求項4】
前記ＸＹ面誤差削減ステップ及び前記Ｚ面誤差削減ステップでは、それぞれ、前記ＸＹ面マップ及び前記Ｚ面マップに対して、前記複数のノードの連結を示すグラフにおけるループを前記車両で周回して同じ点を観測することで累積誤差を削減するループ閉鎖を行うことにより、前記ＸＹ面及び前記Ｚ面での位置誤差を削減する請求項２又は３記載の３次元マップ生成方法。
【請求項5】
前記ＸＹ面誤差削減ステップでは、位相相関によって２つの前記ＸＹ面マップが示す道路を含む地表面のテクスチャ及び形状の一致性を確保することで、前記位置誤差を削減する請求項２～４のいずれか１項に記載の３次元マップ生成方法。
【請求項6】
前記ＸＹ面誤差削減ステップ及び前記Ｚ面誤差削減ステップでは、それぞれ、異なる２つのノードの前記ＸＹ面マップ及び前記Ｚ面マップにおいて共通する領域の一致性である一貫性と、
異なる２つのノードの前記ＸＹ面マップ及び前記Ｚ面マップにおけるエッジの連続性であるコヒーレンシと、
前記ＸＹ面マップ及び前記Ｚ面マップでの位置とＧＰＳで得られる位置との一致性である正確性と
の少なくとも一つを評価するコスト関数を用いて、前記位置誤差を削減する
請求項２～５のいずれか１項に記載の３次元マップ生成方法。
【請求項7】
車両に搭載され、道路を含む環境を検出するＬｉＤＡＲ（Light Detection and Ranging）から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び前記強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成部と、生成された前記３次元データにおける位置誤差を削減することで、３次元マップを生成するマップ生成部とを備え、
前記マップ生成部は、
前記ＸＹ面マップに対してグラフＳＬＡＭ（Simultaneous Localization and Mapping）を実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減部と、
前記ＸＹ面誤差削減部によって位置誤差が削減された前記ＸＹ面マップに対応する前記Ｚ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減部とを有する
３次元マップ生成装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、３次元マップ生成方法及び３次元マップ生成装置に関し、特に、自律走行車両のための３次元マップを生成する技術に関する。
【背景技術】
【0002】
車両の安全な自律走行を可能にするためには、精度の高い３次元マップが必要である。ＧＮＳＳ（Global Navigation Satellite System；汎地球測位航法衛星システム）を用いることで、高い精度での測位が可能であるが、高いコストが必要とされるだけでなく、長いトンネル内、木が密集している箇所、高い建物が多い箇所等では、衛星信号が偏向されたり妨げられたりするため、精度の高い地図の生成が困難となる。このような問題を解決しない場合には、多大な時間、データ及びコストを失って、自律走行している車両をドリフトさせて交通事故を招く恐れがある危険なマップを生成してしまうことになり得る。
【0003】
そこで、従来、走行する車両に搭載したＬｉＤＡＲ（Light Detection and Ranging）から取得した３次元距離画像を示すポイントクラウド（point clouds）に対してＳＬＡＭ（Simultaneous Localization And Mapping；同時位置推定と地図生成）を実行することでマップを生成する技術が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【0004】
【文献】特開２０１９－２０７２２０号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
しかしながら、特許文献１の技術では、ポイントクラウドを用いた３次元画像を伴うマップを生成するものであり、正確なグローバルポジション（ＸＹ面での位置）と正確な標高（Ｚ面での位置）を示す位置精度の高い３次元マップを生成するものではない。
【0006】
より詳しくは、特許文献１の技術は、ポイントクラウドを用いてＳＬＡＭに基づいてマップを生成しているが、実装に関しては非常に複雑である。これは、ポイントクラウドを用いて環境の３次元形状を表すからである。ポイントクラウドは、データサイズがきわめて大きく、長いコースを処理する際には大きな負荷となる。さらに、ポイントクラウドは、車両やその他の動的オブジェクトをマップで表すため、特にＺ方向のトラフィックの流れの影響を受ける。さらに、ＳＬＡＭで良好な結果を得るための最も重要な解決策として、環境機能を利用して位置誤差を補正しているが、ポイントクラウドは非常にスパースであり、２つポイントクラウドを一致させることによってエラーを補正できる可能性は低い。さらに、ポイントクラウドのスパース性のため、反復マッチング手法を使用する必要があるが、そのような手法は、時間の消費という点で非常にコストのかかるプロセスになる。
【0007】
そこで、本発明は、位置精度の高い３次元マップを生成する３次元マップ生成方法及び３次元マップ生成装置を提供することを目的とする。
【課題を解決するための手段】
【0008】
上記目的を達成するために、本発明の一形態に係る３次元マップ生成方法は、車両に搭載され、道路を含む環境を検出するＬｉＤＡＲから取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び前記強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成ステップと、生成された前記３次元データにおける位置誤差を削減することで、３次元マップを生成するマップ生成ステップとを含み、前記マップ生成ステップは、前記ＸＹ面マップに対してグラフＳＬＡＭ（Simultaneous Localization and Mapping）を実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減ステップと、前記ＸＹ面誤差削減ステップによって位置誤差が削減された前記ＸＹ面マップに対応する前記Ｚ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減ステップとを含む。
【0009】
また、上記目的を達成するために、本発明の一形態に係る３次元マップ生成装置は、車両に搭載され、道路を含む環境を検出するＬｉＤＡＲから取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び前記強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成部と、生成された前記３次元データにおける位置誤差を削減することで、３次元マップを生成するマップ生成部とを備え、前記マップ生成部は、前記ＸＹ面マップに対してグラフＳＬＡＭ（Simultaneous Localization and Mapping）を実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減部と、前記ＸＹ面誤差削減部によって位置誤差が削減された前記ＸＹ面マップに対応する前記Ｚ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減部とを有する。
【発明の効果】
【0010】
本発明により、位置精度の高い３次元マップを生成する３次元マップ生成方法及び３次元マップ生成装置が提供される。
【図面の簡単な説明】
【0011】
【発明を実施するための形態】
【0012】
以下、本発明の実施の形態について、図面を用いて詳細に説明する。なお、以下で説明する実施の形態は、いずれも本発明の一具体例を示す。以下の実施の形態で示される数値、形状、手法、構成要素、構成要素の接続形態、ステップ、ステップの順序等は、一例であり、本発明を限定する主旨ではない。また、各図において、実質的に同一の構成については同一の符号を付し、重複する説明は省略又は簡略化する場合がある。また、本明細書において、ＧＮＳＳ及びＧＰＳ（Global Positioning System）は、ＧＮＳＳ／ＩＮＳ－ＲＴＫ（GNSS-aided Inertial Navigation System－Real Time Kinematic）を意味する。
【0013】
図１は、実施の形態に係る３次元マップ生成装置１０の構成を示すブロック図である。３次元マップ生成装置１０は、道路を走行する車両に搭載されたＬｉＤＡＲ１１及びＧＰＳ１２から、それぞれ、ポイントクラウド及び測位データを取得することで、ＸＹ面マップ及びＺ面マップを含む３次元データを生成する３次元データ生成部１３と、生成された３次元データにおける位置誤差を削減することで３次元マップ１５を生成するマップ生成部１４とを備える。
【0014】
３次元データ生成部１３は、ＬｉＤＡＲから取得したポイントクラウドから、道路を含む地表面を表す強度画像（つまり、輝度画像）のフレームで構成されるＸＹ面マップ及び強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する。
【0015】
マップ生成部１４は、３次元データ生成部１３で生成された３次元データのＸＹ面マップに対して、グラフをベースとするＳＬＡＭ（以下、グラフＳＬＡＭという）を実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減部１４ａと、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、グラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減部１４ｂとを有する。
【0016】
なお、３次元データ生成部１３及びマップ生成部１４は、データ処理を実行する機能ブロックであり、プログラムを格納する不揮発性メモリ、データを一時的に格納する揮発性メモリ、プログラムを実行するプロセッサ、周辺装置との入出力を行う入出力インタフェース等を備えるコンピュータ装置によって実現される。
【0017】
図２は、実施の形態に係る３次元マップ生成装置１０の動作手順（つまり、３次元マップ生成方法）を示すフローチャートである。
【0018】
［図２のステップＳ１０］
まず、３次元データ生成部１３は、道路を走行する車両に搭載されたＬｉＤＡＲ１１及びＧＰＳ１２から、それぞれ、ポイントクラウド及び測位データを取得する（Ｓ１０）。
まず、３次元データ生成部１３は、道路を走行する車両に搭載されたＬｉＤＡＲ１１及びＧＰＳ１２から、それぞれ、ポイントクラウド及び測位データを取得する（Ｓ１０）。
【0019】
【0020】
［図２のステップＳ１１］
次に、３次元データ生成部１３は、ＬｉＤＡＲ１１及びＧＰＳ１２から取得したポイントクラウド及び測位データから、ノードの連結からなる３次元データを生成する（図のＳ１１）。より詳しくは、３次元データ生成部１３は、ＬｉＤＡＲ１１から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレーム及び強度画像を構成する画素の標高を示す標高画像のフレームを生成する。そして、３次元データ生成部１３は、所定の矩形領域における強度画像のフレームを連結したものをＸＹ面マップとし、強度画像に対応する標高画像のフレームを連結したものをＺ面マップとし、ＸＹ面マップとＺ面マップとの組をノードとする複数のノードの連結を３次元データとして生成する。このとき、３次元データ生成部１３は、複数のノードのそれぞれに対して、対応するＸＹ面マップが示す矩形領域の特定のコーナーのグローバルポジションを識別子として付与する。
次に、３次元データ生成部１３は、ＬｉＤＡＲ１１及びＧＰＳ１２から取得したポイントクラウド及び測位データから、ノードの連結からなる３次元データを生成する（図のＳ１１）。より詳しくは、３次元データ生成部１３は、ＬｉＤＡＲ１１から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレーム及び強度画像を構成する画素の標高を示す標高画像のフレームを生成する。そして、３次元データ生成部１３は、所定の矩形領域における強度画像のフレームを連結したものをＸＹ面マップとし、強度画像に対応する標高画像のフレームを連結したものをＺ面マップとし、ＸＹ面マップとＺ面マップとの組をノードとする複数のノードの連結を３次元データとして生成する。このとき、３次元データ生成部１３は、複数のノードのそれぞれに対して、対応するＸＹ面マップが示す矩形領域の特定のコーナーのグローバルポジションを識別子として付与する。
【0021】
より詳しくは、３次元データ生成部１３は、複数のノードのそれぞれについて、車両２０に搭載されたＧＰＳ１２から初期位置を取得し、取得された初期位置を基準としてフレームを蓄積していく推測航法（Ｄｅａｄ－Ｒｅｃｋｏｎｉｎｇ）により、当該ノードのＸＹ面マップ及びＺ面マップを生成する。
【0022】
このように、環境を表すために、３次元ポイント分布パターンを用いるのではなく、道路の輝度画像を使用している。輝度画像は、道路のランドマークが静止しており、Ｚ方向の車両表現などの動的オブジェクトの影響を受けずに位置誤差を補正するために利用できる非常に目立つパターンを形成しているからである。そして、グラフＳＬＡＭの適用を２つのフェーズに分割している。一つは、ＸＹ平面であり、もう一つは、Ｚ平面である。ＸＹ平面では、路面とランドマークは、輝度値を用いて、ノード戦略に基づいて広い領域におけるＬＩＤＡＲフレームが蓄積されることから、非常に密（３０ｃｍで）にエンコードされる。これは、１つのまばらなポイントクラウドを使用する代わりに、環境機能を活用するという非常に重要な点である。ノードの輝度画像により、ＸＹ平面内のノード間の位置誤差を大幅に補正できる。したがって、ＸＹ平面にグラフＳＬＡＭを適用し、グローバル座標系でのノードの輝度画像の位置を最適化する。その結果、最適化されたノードの標高画像はＸＹ平面で移動される。したがって、一部の領域を共有するノードは、実際の環境と同じ環境を表すため、同じＺレベルにある必要があるので、Ｚ平面での位置誤差を補正するために、標高画像にグラフＳＬＡＭが適用される。
【0023】
図４は、３次元データ生成部１３が生成する強度画像のフレーム（図４の（ａ））及び標高画像のフレーム（図４の（ｂ））の例を示す図である。３次元データ生成部１３は、ＬｉＤＡＲ１１から取得したポイントクラウドに対して地表面から３０ｃｍの高さでカットすることで、道路を含む地表面を表す強度画像のフレームを生成する。
【0024】
図５は、推測航法によりフレームを蓄積していく３次元データ生成部１３による処理を説明する図である。ここには、車両２０の位置の例（「Vehicle Position」）、ＬｉＤＡＲ１１から取得したポイントクラウドが示す道路、道路の端（「Road edge」）、縁石（Curbs）、フレーム（「One LIDAR Frame」、「1st LIDAR Frame in Node」）の例が示されている。
【0025】
３次元データ生成部１３は、下記式１に示されるように、ＧＰＳ１２から得られる位置を初期位置とし、その初期位置に対して差分としてのフレームを蓄積（つまり、連結）していくことで、例えば、約２００ｍの道路を含む矩形領域のノードを構成するＸＹ面マップ及びＺ面マップを生成する。
【0026】
【数1】
【0027】
ここで、ＸＹＤＲ
ｔは、時刻ｔにおける車両２０のＸＹ面マップでの位置を示し、ｇ（ｖｔ，ｘｙＤＲ
ｔ－１）は、その位置を算出する関数を示し、ｖｔは、車両２０の速度を示し、ｘｙＤＲ
ｔ－１は、時刻（ｔ－１）における車両２０のＸＹ面マップでの位置を示し、Δｔは、直前の位置からの経過時間を示す。また、ＺＤＲ
ｔは、時刻ｔにおける車両２０のＺ面マップでの位置を示し、ｇ（ｖｔ，ｚＤＲ
ｔ－１）は、その位置を算出する関数を示し、ｚＤＲ
ｔ－１は、時刻（ｔ－１）における車両２０のＺ面マップでの位置を示す。
【0028】
このような矩形領域のノード（ＸＹ面マップ及びＺ面マップ）の生成は、複数のノード分だけ繰り返される。
【0029】
このように、ＧＮＳＳを使用して車両の位置を推定する代わりに、衛星信号の精度が低いために生じ得るローカルジャンプを回避するために、推測航法を使用して各ノード内のＬＩＤＡＲフレームを蓄積する。推測航法によって、各ノード内で非常にスムーズな車両軌道が実現される。よって、ＬＩＤＡＲフレームはスムーズに蓄積され、各ノード内の道路状況が保持される。
【0030】
図６は、３次元データ生成部１３によって生成されるＸＹ面マップの例を示す図である。図６の（ａ）及び（ｂ）に示される２つのノードのＸＹ面マップは、車両２０の走行軌跡におけるＸＹグローバル座標系でのトップレフトコーナーの位置（各図に示される「Ｘｍｉｎ」及び「Ｙｍａｘ」）が識別子として用いられて結合されることで、図６の（ｃ）に示されるような全体を示すＸＹ面マップが生成される。
【0031】
図７は、３次元データ生成部１３によって生成されたＺ面マップの例（ここでは、２つのノードのＺ面マップの例（図７の（ａ）及び（ｂ））を示す図である。本図に示されるように、本実施の形態では、各ノードのＺ面マップは、車両２０の走行軌跡におけるＺグローバル座標系での非ゼロの標高の平均値（図７の（ａ）では１５３．２２ｍ、図７の（ｂ９では１５４．０６ｍ）が識別子として用いられる。
【0032】
図８は、３次元データ生成部１３によって生成される３次元データの例を示す図である。本図に示されるように、３次元データは、識別子（ここでは、Ｎｏｄｅ１、２、・・、９）で識別される各ノードのＸＹ面マップ及びＺ面マップ（ここでは、Ｎｏｄｅ５）が連結されて構成されるＬｉＤＡＲマップである。より詳しくは、本図では、強度マップ（つまり、ＸＹ面マップ）と標高マップ（つまり、Ｚ面マップ）の２つの画像が含まれている。つまり、各ノードには２種類の画像（標高と強度）が含まれている。
【0033】
［図２のステップＳ１２］
次に、マップ生成部１４のＸＹ面誤差削減部１４ａは、３次元データ生成部１３が生成したＸＹ面マップに対してグラフＳＬＡＭを実行することで、ＸＹ面での位置誤差を削減する（図２のＳ１２）。具体的には、以下のようなステップＳ１２ａ～Ｓ１２ｃを行う。
次に、マップ生成部１４のＸＹ面誤差削減部１４ａは、３次元データ生成部１３が生成したＸＹ面マップに対してグラフＳＬＡＭを実行することで、ＸＹ面での位置誤差を削減する（図２のＳ１２）。具体的には、以下のようなステップＳ１２ａ～Ｓ１２ｃを行う。
【0034】
［図２のステップＳ１２ａ］
ＸＹ面誤差削減部１４ａは、ＸＹ面マップに対して、３次元データ生成部１３が生成した複数のノードの連結を示すグラフにおけるループを車両２０で周回して同じ点を観測することで累積誤差を削減する処理であるグラフＳＬＡＭのループ閉鎖を行うことにより、ＸＹ面での位置誤差を削減する（図２のＳ１２ａ）。
ＸＹ面誤差削減部１４ａは、ＸＹ面マップに対して、３次元データ生成部１３が生成した複数のノードの連結を示すグラフにおけるループを車両２０で周回して同じ点を観測することで累積誤差を削減する処理であるグラフＳＬＡＭのループ閉鎖を行うことにより、ＸＹ面での位置誤差を削減する（図２のＳ１２ａ）。
【0035】
図９は、３次元データ生成部１３が行うグラフＳＬＡＭのループ閉鎖を説明する図である。いま、車両２０が位置Ｐ０からスタートし、位置Ｐ１、Ｐ２、・・、Ｐ１１と順に移動したとする。このようなケースでは、ＧＰＳ１２を用いて計測したスタートの位置Ｐ０は、正確であるが、その後、差分としてのフレームが蓄積されることから、スタート位置に近い位置（例えば、Ｐ１及びＰ２）では高い位置精度が維持されるが、車両２０の移動が進むにつれて累積誤差が増す。車両は、位置Ｐ９では、位置Ｐ３のエリアに戻っている。これは、位置Ｐ３と位置Ｐ９の間の位置が車両の軌道に従ってループを形成するため、ＸＹ平面でのループ閉鎖イベント（あるいは、単に、ループ閉鎖）と呼ばれる。ただし、位置Ｐ６後の累積誤差のため、位置Ｐ３とＰ位置９との間に相対位置誤差がある。位置Ｐ３と位置Ｐ９で検知された環境特性を一致させることにより、相対位置誤差が補正され、ループ内の位置全体（位置Ｐ４…Ｐ８）の精度が向上される。これがグラフＳＬＡＭのループ閉鎖を用いたＸＹ面での位置誤差の削減である。
【0036】
本実施の形態では、車両の位置を使用する代わりに、グローバル座標系のトップレフトコーナーの位置を最適化している。これは、推測航法に基づいてＬＩＤＡＲフレームを円滑に蓄積することを保証するノード戦略を用いているからである。
【0037】
［図２のステップＳ１２ｂ］
上記ループ閉鎖における位置Ｐ３と位置Ｐ９のケース、あるいは、隣接する２つのノードにおける共通領域のように、２つのノードの強度画像を一致させる場合には、ＸＹ面誤差削減部１４ａは、位相相関によって２つのＸＹ面マップが示す道路を含む地表面のテクスチャ及び形状の一致性を確保する処理（以下、この処理を「位相相関処理」という）を行うことで、位置誤差を削減する（図２のＳ１２ｂ）。位相相関とは、位相限定相関法とも呼ばれる手法であり、フーリエ変換後の位相スペクトルを利用して画像のマッチングを行う方法である。
上記ループ閉鎖における位置Ｐ３と位置Ｐ９のケース、あるいは、隣接する２つのノードにおける共通領域のように、２つのノードの強度画像を一致させる場合には、ＸＹ面誤差削減部１４ａは、位相相関によって２つのＸＹ面マップが示す道路を含む地表面のテクスチャ及び形状の一致性を確保する処理（以下、この処理を「位相相関処理」という）を行うことで、位置誤差を削減する（図２のＳ１２ｂ）。位相相関とは、位相限定相関法とも呼ばれる手法であり、フーリエ変換後の位相スペクトルを利用して画像のマッチングを行う方法である。
【0038】
各ノードは、実際の路面の広い領域をエンコードしたものである。このことから、３次元の疎なポイントクラウドを使用する代わりに、非常に密にランドマークを表現することで、環境の特徴を活用できる。このような密集した表現、広い路面、３０ｃｍのランドマークのみの表現により、従来のポイントクラウドでのマッチング手法の代わりに、高レベルの画像処理テクニックを使用して強度画像をマッチングできる。したがって、位相相関を使用して、強度画像に対して、ノード間の相対位置を推定できる。さらに、従来のポイントクラウドでのマッチング手法は、通常、反復検索によって２つのポイントクラウドを位置合わせをする。これは、３次元パターンとポイントクラウドとに希少性が存在することが原因である。したがって、特に走行のコースが長く、多くのループ閉鎖が含まれている場合、ポイントクラウドでのマッチング手法は、時間の消費という点で非常にコストのかかるプロセスになる。一方、本実施の形態における位相相関は、非反復的な手法であり、処理時間が大幅に短縮される。
【0039】
図１０は、ＸＹ面誤差削減部１４ａによる位相相関処理を説明する図である。ここでは、隣接する２つのノードＡ及びＢの強度画像（図１０の（ａ）及び（ｂ））、それら２つの強度画像の共通領域（白枠で囲まれた領域）に対して位相相関処理を施して得られた強度画像（図１０の（ｃ））、及び、ＧＮＳＳを用いて得られた正確な画像（図１０の（ｄ））の例が示されている。位相相関は、複数のノードにおける相対位置を推定するのにロバストであり、かつ、信頼性が高く、相対位置誤差を削減することができる。
【0040】
より詳しくは、図１０の（ａ）及び（ｂ）は、それぞれ、２つのノードＡ及びＢを示している。図１０の（ｃ）は、位相相関に基づく２つのノードのマッチング結果を示している。図１０の（ｄ）は、左上隅を使用したＧＮＳＳに基づくノードＡ及びノードＢの単純な組み合わせを示している。ノードＡ及びノードＢは、ＧＮＳＳが衛星から高品質の信号を受信できるエリア、つまり、非常に正確なオープンスカイのエリアに属している。したがって、ＧＮＳＳによる図１０の（ｄ）の２つのノードの組み合わせは、リファレンス（真実の地面）と見なすことができる。位相相関によれば、ノードＡとノードＢの左上隅についての情報がなくても、ＧＮＳＳと同じマップ品質が実現される。これは、位相相関によって、オープンスカイ環境でのノード間の相対位置と同程度のＧＮＳＳ推定が実現されることになる。さらに、衛星信号が偏向または妨害される可能性のある厳しい環境での相対位置を完全に推定するためにも位相相関が役立っている。位相相関がノードの輝度画像によって広く／密に表される環境の特徴に依存しているからである。そして、環境の特徴は、静的で、固定的で、衛星信号の品質または走行距離に関連しているからである。
【0041】
［図２のステップＳ１２ｃ］
また、ＸＹ面誤差削減部１４ａは、次の３つを評価するコスト関数を用いて、ＸＹ面での位置誤差（相対位置誤差及びグローバル位置誤差）を削減する最適化を行う（図２のＳ１２ｃ）。（１）一貫性、つまり、異なる２つのノードのＸＹ面マップにおいて共通する領域の一致性である。（２）コヒーレンシ、つまり、異なる２つのノードのＸＹ面マップにおける連続性である。（３）正確性、つまり、ＸＹ面マップでの位置とＧＰＳで得られる位置との一致性である。
また、ＸＹ面誤差削減部１４ａは、次の３つを評価するコスト関数を用いて、ＸＹ面での位置誤差（相対位置誤差及びグローバル位置誤差）を削減する最適化を行う（図２のＳ１２ｃ）。（１）一貫性、つまり、異なる２つのノードのＸＹ面マップにおいて共通する領域の一致性である。（２）コヒーレンシ、つまり、異なる２つのノードのＸＹ面マップにおける連続性である。（３）正確性、つまり、ＸＹ面マップでの位置とＧＰＳで得られる位置との一致性である。
【0042】
図１１は、コスト関数で評価される一貫性を説明する図である。本図に示されるように、ループ閉鎖における同じ位置を示す２つのノード（ここでは、ノードＮ１及びＮｉ＋２）の強度画像は、それら２つのノードの強度画像間での一致性（差分が小さいこと）が、一貫性として、コスト関数で評価される。具体的には、一貫性の評価は、下記式２で示される画像エッジＥＩｍｇ
ｉ，ｊで示される。なお、エッジとは、評価の数学的表現である。
【0043】
【数2】
【0044】
ここで、ＥＩｍｇ
ｉ，ｊは、相対位置を補償してループ閉鎖を構成する２つのノードＮｉ及びＮｊの共通領域を評価する画像エッジを示し、ｆ（）は、相対位置を計算する関数であり、ＸＮｉは、ノードＮｉでのＸ位置を示し、ＸＮｊは、ノードＮｊでのＸ位置を示し、ＸＩｍｇ
Ｎｉ，ｊは、強度画像におけるノードＮｉ及びＮｊの相対位置を示し、Ω（）は、ノードＮｉ及びＮｊ間の位相相関の信頼性を表す共分散誤差である。この共分散誤差は、位相相関を算出する相関行列から得られる。このような一貫性の評価は、ＸＹ面マップでの相対位置誤差の削減に貢献する。
【0045】
図１２は、コスト関数で評価されるコヒーレンシを説明する図である。本図に示されるように、隣接する２つのノード（ここでは、ノードＮ０及びＮ１）における共通領域の強度画像は、それら２つのノードの強度画像間での連続性が、コヒーレンシとして、コスト関数で評価される。連続性は、例えば、道路のコンテキストである。具体的には、コヒーレンシの評価は、下記式３で示される連続性エッジＥＩｍｇ
ｉ，ｉ－１で示される。
【0046】
【数3】
【0047】
ここで、ＥＤＲ
ｉ，ｉ－１は、隣接する２つのノードＮｉ及びＮｉ－１の共通領域を評価する連続性エッジを示し、ｆ（）は、相対位置を計算する関数であり、ＸＮｉは、ノードＮｉでのＸ位置を示し、ＸＮｉ－１は、ノードＮｉ－１でのＸ位置を示し、ＸＩｍｇ
Ｎｉ，ｉ－１は、強度画像におけるノードＮｉ及びＮｉ－１の相対位置を示し、Σ（）は、車両２０のノードＮｉ及びＮｉ－１内での速度の分散から推定できる共分散誤差を示す。
【0048】
このようなコヒーレンシの評価は、２つのノードにおける道路のコンテキストの連続性を維持するのに重要な役割を果たし、ＸＹ面マップでの相対位置誤差の削減に貢献する。
【0049】
図１３は、コスト関数で評価される正確性を説明する図である。本図に示されるように、ＸＹ面マップでの位置とＧＰＳで得られる位置の一致性（ここでは、ノードＮ１及びＮｉ＋２のそれぞれにおけるＸＹ面マップでの位置とＧＰＳ１２で得られる位置との一致性）が、正確性として、コスト関数で評価される。具体的には、正確性の評価は、下記式４で示されるアンカリング・エッジＸＹＧＰＳ
ｉで示される。
【0050】
【数4】
【0051】
ここで、ＸＹＧＰＳ
ｉは、ノードＮｉの正確性を評価するアンカリング・エッジを示し、ＸＮｉは、ノードＮｉのＸＹ面マップでのＸ位置を示し、ＸＧＰＳ
Ｎｉは、ノードＮｉのＧＰＳで得られるＸ位置を示し、Γ（）は、ＧＰＳ１２を用いて得られる共分散誤差を示す。このような正確性の評価は、ＸＹ面マップでのグローバル位置誤差の削減に貢献する。３次元マップは、グローバル座標系（絶対座標系）で生成され表現される必要がある。そのために、ＧＮＳＳに基づくアンカリング・エッジが使用される。
【0052】
［図２のステップＳ１３］
次に、マップ生成部１４のＺ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、グラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減する（図２のＳ１３）。具体的には、以下のようなステップＳ１３ａ～Ｓ１３ｂを行う。
次に、マップ生成部１４のＺ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、グラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減する（図２のＳ１３）。具体的には、以下のようなステップＳ１３ａ～Ｓ１３ｂを行う。
【0053】
［図２のステップＳ１３ａ］
Ｚ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、ＸＹ面誤差削減部１４ａと同様の手法で、グラフＳＬＡＭのループ閉鎖を行うことにより、Ｚ面での位置誤差を削減する（図２のＳ１３ａ）。
Ｚ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、ＸＹ面誤差削減部１４ａと同様の手法で、グラフＳＬＡＭのループ閉鎖を行うことにより、Ｚ面での位置誤差を削減する（図２のＳ１３ａ）。
【0054】
［図２のステップＳ１３ｂ］
また、Ｚ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、ＸＹ面誤差削減部１４ａと同様の手法で、次の３つの少なくとも一つを評価するコスト関数を用いて、Ｚ面での位置誤差（相対位置誤差及びグローバル位置誤差）を削減する最適化を行う（図２のＳ１３ｂ）。（１）一貫性、つまり、異なる２つのノードのＺ面マップにおいて共通する領域の一致性である。（２）コヒーレンシ、つまり、異なる２つのノードのＺ面マップにおける連続性である。（３）正確性、つまり、Ｚ面マップでの位置とＧＰＳで得られる位置との一致性である。
また、Ｚ面誤差削減部１４ｂは、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して、ＸＹ面誤差削減部１４ａと同様の手法で、次の３つの少なくとも一つを評価するコスト関数を用いて、Ｚ面での位置誤差（相対位置誤差及びグローバル位置誤差）を削減する最適化を行う（図２のＳ１３ｂ）。（１）一貫性、つまり、異なる２つのノードのＺ面マップにおいて共通する領域の一致性である。（２）コヒーレンシ、つまり、異なる２つのノードのＺ面マップにおける連続性である。（３）正確性、つまり、Ｚ面マップでの位置とＧＰＳで得られる位置との一致性である。
【0055】
なお、Ｚ面での一貫性の評価は、下記式５で示される画像エッジＺＩｍｇ
ｉ，ｊで示されるが、そのための最適化の処理は、ＸＹ面誤差削減部１４ａによるＸＹ面マップに対する一貫性の処理によって、同時に実現される。異なる２つのノードのＸＹ面マップにおいて共通する領域の一致性が確保される処理によって、同時に、それら２つのノードのＺ面マップにおいて共通する領域の一致性が確保されるからである。
【0056】
【数5】
【0057】
この式５における各表記は、式２の表記におけるＸをＺに置き換えたものに相当する。このような一貫性の評価は、Ｚ面マップでの相対位置誤差の削減に貢献する。
【0058】
また、コヒーレンシの評価は、下記式６で示される連続性エッジＺＤＲ
ｉ，ｉ－１で示される。
【0059】
【数6】
【0060】
この式６における各表記は、式３の表記におけるＸをＺに置き換えたものに相当する。このようなコヒーレンシの評価は、２つのノードにおける標高の連続性を維持するのに重要な役割を果たし、Ｚ面マップでの相対位置誤差の削減に貢献する。
【0061】
また、正確性の評価は、下記式７で示されるアンカリング・エッジＺＧＰＳ
ｉで示される。
【0062】
【数7】
【0063】
この式７における各表記は、式４の表記におけるＸをＺに置き換えたものに相当する。このような正確性の評価は、Ｚ面マップでのグローバル位置誤差の削減に貢献する。
【0064】
［図２のステップＳ１４］
最後に、マップ生成部１４は、ＸＹ面誤差削減部１４ａ及びＺ面誤差削減部１４ｂによって位置誤差が削減された３次元データを、最終的な３次元マップ１５として、外部の記憶装置又はディスプレイ等の出力装置へ出力する。
最後に、マップ生成部１４は、ＸＹ面誤差削減部１４ａ及びＺ面誤差削減部１４ｂによって位置誤差が削減された３次元データを、最終的な３次元マップ１５として、外部の記憶装置又はディスプレイ等の出力装置へ出力する。
【0065】
図１４は、図２のステップＳ１２ｃ及びＳ１３ｂの処理をまとめた図である。本図に示されるように、３次元データ生成部１３で生成された３次元データ（ＸＹ面マップ及びＺ面マップ）に対して、まず、マップ生成部１４のＸＹ面誤差削減部１４ａにより、ＸＹ面マップに対して、コスト関数を用いた最適化を実行することでＸＹ面での位置誤差を削減した後に（図２のＳ１２ｃ）、位置誤差が削減されたＸＹ面マップに対応する（つまり、投影される）Ｚ面マップに対して、Ｚ面誤差削減部１４ｂにより、コスト関数を用いた最適化を実行することで、Ｚ面での位置誤差を削減し（図２のＳ１３ｂ）、最終的な３次元マップ１５として出力する。
【0066】
図１５Ａは、図１４に示される処理手順によるメリットを説明する図である。ここでは、図１４のマップにおいて複数の他のノードと関係性を持つ１つのノードの単純な例が示されている。これらの関係性とは、ＸＹ平面にグラフＳＬＡＭを適用し、次にＺ平面にグラフＳＬＡＭを適用することで、ノードの位置に影響を与えるものである。本図に示されるように、本実施の形態に係る３次元マップ生成装置１０によれば、まず、ＸＹ面マップに対して３種類の評価を行うコスト関数を用いた最適化を実行する（上述した３種類のＸＹ面でのエッジの合計を最小化する）ことで、ＸＹ面での位置誤差を削減するためのＸＹオフセットが得られる（図１５Ａの（ａ）の１及び２行目、図１５Ａの（ｂ））。各ノードには、トップレフトコーナーの座標に追加されるｘおよびｙ方向の２つのオフセットがある。したがって、このようなＸＹグラフＳＬＡＭオフセットに基づいて絶対座標系で路面を移動させることを意味する。その後、ノード全体を組み合わせてマップが生成される。
【0067】
続いて、位置誤差が削減されたＸＹ面マップに対応する（つまり、投影される）Ｚ面マップに対して３種類の評価を行うコスト関数を用いた最適化を実行する（上述した３種類のＺ面でのエッジの合計を最小化する）ことで、Ｚ面での位置誤差を削減するためのＺオフセットが得られる（図１５Ａの（ａ）の３及び４行目、図１５Ａの（ｃ））。ＸＹグラフＳＬＡＭオフセットに基づいてＺ平面でマップを修正する手順は、標高画像にグラフＳＬＡＭを適用することによって行われる。したがって、Ｚオフセットのセットが取得され、各ノードには１つの標高オフセットが得られ、これが標高画像のピクセル全体に追加される。これは、ＺグラフＳＬＡＭオフセットに基づいて標高画像をＺ方向に移動することを意味する。次に、ノード全体を組み合わせてＺ面マップを生成する。これらのステップにより、完全なマップが取得される。
【0068】
これにより、ＸＹ面での誤差がＺ面での誤差に累積されるという不具合が抑制され、正確なグローバルポジション（ＸＹ面での位置）及び正確な標高（Ｚ面での位置）を示す位置精度の高い３次元マップが生成される。
【0069】
図１５Ｂは、図１４に示される処理手順によるメリットを説明する別の図である。ここでは、２つのノード間に着目して、図１４に示される手法が説明されている。図１５Ｂの（ａ）は、２つのノードＡ及びＢの強度画像及び標高画像の例を示す。図１５Ｂの（ｂ）は、ＸＹ平面とＺ平面におけるノード間の３次元偏差の問題を示している。ＸＹ面度マップ内の道路のランドマークの複製が示されるように、２つのノード間においてｘ方向とｙ方向（ＸＹ平面）に相対位置エラーが生じている。さらに、Ｚ面マップで異なるｚレベルの路面が示されるように、ｚ方向の相対位置誤差も示されている。図１５Ｂの（ｃ）は、強度画像のＸＹ平面にグラフＳＬＡＭを適用した後の問題を示している。ｘ方向及びｙ方向の相対位置誤差が補正され、ゴースト効果がマップから除去されている。その結果、図１５Ｂの（ｄ）に示すように、標高画像にグラフＳＬＡＭを適用することにより、２つの道路間の高度誤差（ｚ方向の相対位置誤差）をできるだけ小さくすることができる。現実の世界では、２つの道路は同じ環境を表すものであることから、同じＺレベルになっている。
【0070】
次に、本実施の形態に係る３次元マップ生成装置１０を用いて３次元マップを生成する実験を行ったので、図を用いて説明する。
【0071】
図１６は、実験に用いた車両２０の走行軌跡を示す図である。図１６の（ａ）は、走行軌跡と走行軌跡における特徴的な箇所を示し、図１６の（ｂ）は、走行軌跡を描いた地図を示す。本実験では、金沢市の香林坊町において、車両２０で２５ｋｍの総距離を走行することで得られたポイントクラウドから、３０５個のノード及び２８６個のループ閉鎖エッジを含む３次元マップを生成した。図１６の（ａ）に示されるように、走行軌跡には、高層ビルが建ち並ぶ箇所、及び、木が密集している箇所が含まれている。
【0072】
図１７は、３次元マップ生成装置１０が生成したＸＹ面マップの位置精度の高さを示す図である。図１７の（ａ）は、ＧＮＳＳを用いて生成したＸＹ面マップの各ノードにおけるＸ方向の標準偏差（ｍ）を示し（つまり、各ノードのトップレフトコーナーのＸ座標の予測精度を示し）、図１７の（ｂ）は、３次元マップ生成装置１０が生成したＸＹ面マップの各ノードにおけるＸ方向のオフセット（ｍ）を示し（つまり、各ノードのトップレフトコーナーのＸ座標に加算されたグラフＳＬＡＭオフセットを示し）、図１７の（ｃ）は、ＧＮＳＳを用いて生成したＸＹ面マップの各ノードにおけるＹ方向の標準偏差（ｍ）を示し（つまり、各ノードのトップレフトコーナーのＹ座標の予測精度を示し）、図１７の（ｄ）は、３次元マップ生成装置１０が生成したＸＹ面マップの各ノードにおけるＹ方向のオフセット（ｍ）を示す（つまり、各ノードのトップレフトコーナーのＹ座標に加算されたグラフＳＬＡＭオフセットを示す）。
【0073】
図１７の（ａ）及び（ｂ）を比較して分かるように、３次元マップ生成装置１０によれば、グラフＳＬＡＭオフセットの加算により、Ｘ方向の低精度なノードが検出され、それらのノードに対して、絶対座標系におけるグローバルマップ精度が高められている。Ｙ方向についても、図１７の（ｃ）及び（ｄ）を比較して分かるように、絶対座標系におけるグローバルマップ精度が高められている。
【0074】
図１８は、木が密集している箇所において３次元マップ生成装置１０が生成したＸＹ面マップとＧＮＳＳを用いて生成したＸＹ面マップとの比較を示す図である。図１８の（ａ）は、木が密集している箇所を撮影した画像を示し、図１８の（ｂ）は、図１８の（ａ）で示される箇所についてＧＮＳＳを用いて生成したＸＹ面マップの例を示し、図１８の（ｃ）は、図１８の（ａ）で示される箇所について３次元マップ生成装置１０が生成したＸＹ面マップの例を示す。
【0075】
図１８の（ｂ）から分かるように、ＧＮＳＳを用いて生成したＸＹ面マップでは、衛星信号が密集している木で妨げられるために、道路の車線の重複やゴースト等の相対位置誤差が生じている。また、道路が現実と異なって２箇所に示されているように、グローバル位置誤差が生じている。これに対して、図１８の（ｃ）から分かるように、３次元マップ生成装置１０で生成されたＸＹ面マップでは、図１８の（ｂ）における相対位置誤差及びグローバル位置誤差が大幅かつ完全に削減される。
【0076】
図１９は、３次元マップ生成装置１０が生成したＺ面マップの位置精度の高さを示す図である。図１９の（ａ）は、ＧＮＳＳを用いて生成したＸＹ面マップの各ノードにおけるＺ方向の標準偏差（ｍ）を示し（つまり、各ノードのトップレフトコーナーのＺ座標の予測精度を示し）、図１９の（ｂ）は、３次元マップ生成装置１０が生成したＺ面マップの各ノードにおけるＺ方向のオフセット（ｍ）を示す（つまり、各ノードのトップレフトコーナーのＺ座標に加算されたグラフＳＬＡＭオフセットを示す）。図１９の（ａ）及び（ｂ）を比較して分かるように、３次元マップ生成装置１０によれば、グラフＳＬＡＭオフセットの加算により、Ｚ方向の低精度なノードが検出され、それらのノードに対して、絶対座標系におけるグローバルマップ精度が高められる。
【0077】
図２０は、図１８に示される木が密集している箇所において３次元マップ生成装置１０が生成したＺ面マップとＧＮＳＳを用いて生成したＺ面マップとの比較を示す図である。図２０の（ａ）は、比較の対象となる２つのノード６９及び２３９の強度画像を示している。点線は、車両の軌跡を示している。ノード６９における長方形の枠で囲まれた領域の座標は、ＸＹ平面のグラフＳＬＡＭオフセットに基づいて、ノード２３９に投影される。したがって、ノード６９における車両の軌道は、投影されたエリア座標に基づいてノード２３９に投影される。
【0078】
図２０の（ｂ）は、同一場所である２つの異なるノード（ノード６９及びノード２３９）について３次元マップ生成装置１０が生成したＺ面マップの標高プロファイル（車両２０の移動に伴う標高の変化）及びＧＮＳＳを用いて生成したＺ面マップの標高プロファイルを示す。本図において、破線（NodeSlam69）及び点線（NodeSlam239）は、それぞれ、３次元マップ生成装置１０が生成したノード６９及びノード２３９のＺ面マップの標高プロファイルを示す。実線（NodeGPS69）及び一点鎖線（NodeGPS69）は、それぞれ、ＧＮＳＳを用いて生成したノード６９及びノード２３９のＺ面マップの標高プロファイルを示す。
【0079】
図２０の（ｃ）は、３次元マップ生成装置１０が生成したＺ面マップとＧＮＳＳを用いて生成したＺ面マップについて、移動軌跡の各箇所におけるノード６９での標高とノード２３９での標高との差を示す。本図において、破線（DiffSlam）は、３次元マップ生成装置１０が生成したＺ面マップでの移動軌跡の各箇所におけるノード６９での標高とノード２３９での標高との差を示し、実線（DiffGPS）は、ＧＮＳＳを用いて生成したＺ面マップでの移動軌跡の各箇所におけるノード６９での標高とノード２３９での標高との差を示す。
【0080】
上述したようにノード６９とノード２３９とは同一場所であるから、標高の差はゼロであるべきところ、ＧＮＳＳを用いて生成したＺ面マップでは、大きな値になっているのに対し、３次元マップ生成装置１０が生成したＺ面マップでは、極めて小さな値となっている。このことから、本実施の形態に係る３次元マップ生成装置１０によれば、グラフＳＬＡＭを適用したうえでコスト関数による最適化が行われるので、Ｚ面での位置誤差が小さい正確なＺ面マップが生成されることが分かる。
【0081】
なお、図２０におけるＧＮＳＳを用いて生成したＺ面マップでの誤差は、ＸＹ面マップにグラフＳＬＡＭを適用した後のものであり、ＧＮＳＳを用いて生成したＸＹ面マップでの誤差が無視されている。よって、もし、ＧＮＳＳを用いて生成したＸＹ面マップでの誤差を勘案した場合には、ＧＮＳＳを用いて生成したＺ面マップでの誤差は、もっと大きい値となる。
【0082】
以上のように、本実施の形態に係る３次元マップ生成方法は、車両２０に搭載され、道路を含む環境を検出するＬｉＤＡＲ１１から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成ステップ（Ｓ１０、Ｓ１１）と、生成された３次元データにおける位置誤差を削減することで３次元マップを生成するマップ生成ステップ（Ｓ１２、Ｓ１３）とを含み、マップ生成ステップは、ＸＹ面マップに対してグラフＳＬＡＭを実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減ステップ（Ｓ１２）と、ＸＹ面誤差削減ステップ（Ｓ１２）によって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減ステップ（Ｓ１３）とを含む。
【0083】
これにより、ＸＹ面での位置誤差が削減された後に、位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して位置誤差を削減する処理が行われるので、ＸＹ面での誤差がＺ面での誤差に累積されるという不具合が抑制され、正確なグローバルポジション（ＸＹ面での位置）及び正確な標高（Ｚ面での位置）を示す位置精度の高い３次元マップが生成される。
【0084】
また、３次元データ生成ステップ（Ｓ１１）では、所定の矩形領域における強度画像をＸＹ面マップとし、強度画像に対応する標高画像をＺ面マップとし、ＸＹ面マップとＺ面マップとの組をノードとする複数のノードの連結を３次元データとして生成し、複数のノードのそれぞれに対して、対応するＸＹ面マップが示す矩形領域の特定のコーナーのグローバルポジションを識別子として付与する。これにより、広域の３次元マップがノードの連結からなるグラフで表現されるので、グラフＳＬＡＭを適用した位置誤差の削減が可能になる。
【0085】
また、３次元データ生成ステップ（Ｓ１１）では、複数のノードのそれぞれについて、車両２０に搭載されたＧＰＳから初期位置を取得し、取得された初期位置を基準としてフレームを蓄積していく推測航法により、当該ノードのＸＹ面マップ及びＺ面マップを生成する。これにより、常に衛星信号を必要とするＧＮＳＳを用いた地図生成に比べ、長いトンネル内、木が密集している箇所、高い建物が多い箇所等の衛星信号が届きにくい箇所であっても、位置精度の高い３次元マップの生成が可能になる。
【0086】
また、ＸＹ面誤差削減ステップ（Ｓ１２）及びＺ面誤差削減ステップ（Ｓ１３）では、それぞれ、ＸＹ面マップ及びＺ面マップに対して、複数のノードの連結を示すグラフにおけるループを車両２０で周回して同じ点を観測することで累積誤差を削減するループ閉鎖を行うことにより、ＸＹ面及びＺ面での位置誤差を削減する。これにより、ＸＹ面及びＺ面での累積的な位置誤差が削減され、位置精度の高い３次元マップの生成が可能になる。
【0087】
また、ＸＹ面誤差削減ステップ（Ｓ１２）では、位相相関によって２つのＸＹ面マップが示す道路を含む地表面のテクスチャ及び形状の一致性を確保することで、位置誤差を削減する。これにより、複数のノードにおける相対位置の推定にロバストであり、かつ、信頼性が高い位相相関が用いられるので、相対位置誤差が削減された位置精度の高い３次元マップの生成が可能になる。
【0088】
また、ＸＹ面誤差削減ステップ（Ｓ１２）及びＺ面誤差削減ステップ（Ｓ１３）では、それぞれ、異なる２つのノードのＸＹ面マップ及びＺ面マップにおいて共通する領域の一致性である一貫性と、異なる２つのノードのＸＹ面マップ及びＺ面マップにおけるエッジの連続性であるコヒーレンシと、ＸＹ面マップ及びＺ面マップでの位置とＧＰＳで得られる位置との一致性である正確性との少なくとも一つを評価するコスト関数を用いて、位置誤差を削減する。これにより、ＸＹ面及びＺ面で、相対位置誤差及びグローバル位置誤差を削減する最適化が行われるので、位置精度の高い３次元マップの生成が可能になる。
【0089】
また、本実施の形態に係る３次元マップ生成装置１０は、車両２０に搭載され、道路を含む環境を検出するＬｉＤＡＲ１１から取得したポイントクラウドから、道路を含む地表面を表す強度画像のフレームで構成されるＸＹ面マップ及び強度画像を構成する画素の標高を示す標高画像のフレームで構成されるＺ面マップを含む３次元データを生成する３次元データ生成部１３と、生成された３次元データにおける位置誤差を削減することで３次元マップを生成するマップ生成部１４とを備え、マップ生成部１４は、ＸＹ面マップに対してグラフＳＬＡＭを実行することで、ＸＹ面での位置誤差を削減するＸＹ面誤差削減部１４ａと、ＸＹ面誤差削減部１４ａによって位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対してグラフＳＬＡＭを実行することで、Ｚ面での位置誤差を削減するＺ面誤差削減部１４ｂとを有する。
【0090】
これにより、ＸＹ面での位置誤差が削減された後に、位置誤差が削減されたＸＹ面マップに対応するＺ面マップに対して位置誤差を削減する処理が行われるので、ＸＹ面での誤差がＺ面での誤差に累積されるという不具合が抑制され、正確なグローバルポジション（ＸＹ面での位置）及び正確な標高（Ｚ面での位置）を示す位置精度の高い３次元マップが生成される。
【0091】
このように、本実施の形態に係る３次元マップ生成装置１０及びその方法によれば、道路をノードに分割する手法により、ループ閉鎖の検出、エッジ計算、データの保存などのデータ関連付けを実現するための処理時間が短縮される。さらに、本実施の形態では、最適化処理において、ノードのトップレフトコーナーを処理するため、３次元マップを効率的に再生成できる。したがって、ノードの画像は、膨大な数のポイントクラウドを再蓄積するのではなく、取得したオフセットに基づいてグローバル座標系でのみシフトされる。ノードの画像は、道路についての広い領域と周囲の環境とを相対的にエンコードしている。反射率の値により、描かれたランドマークが目立つマッチングパターンが実現される。これにより、位相相関法の適用が可能になり、ノード間の共通領域の誤検出が減少する。
【0092】
処理時間の短縮については、図１６を用いて説明したように、２５ｋｍを表す３０５のノードで構成されるコースは、収集されたＬＩＤＡＲフレームの数が６１７５５個のポイントクラウドで構成されるが、ループ閉鎖エッジの数は２８６であり、ノード間のループ閉鎖を検出する処理時間は４４秒、エッジの計算は約５７分、最適化処理の実行時間は３０秒で、ノードの画像を再調整して３次元マップを生成する時間は１７秒であった。従来の方法によれば、６１７５５のポイントクラウド間の正しい対応を検出するのが困難であり、２つのポイントクラウド間で反復スキャンマッチングテクニックを適用し（２つのポイントクラウドごとに１７秒ほど要する）、最適化プロセスを実行し、ポイントクラウドを再蓄積してマップを生成するには、膨大な処理時間を要する。
【0093】
以上、本発明の３次元マップ生成方法及び３次元マップ生成装置１０について、実施の形態に基づいて説明したが、本発明は、この実施の形態に限定されるものではない。本発明の主旨を逸脱しない限り、当業者が思いつく各種変形を本実施の形態に施したものや、実施の形態における一部の構成要素を組み合わせて構築される別の形態も、本発明の範囲内に含まれる。
【0094】
例えば、車両２０に、道路を含む環境を撮影するカメラを搭載し、３次元マップ生成装置１０は、ＬｉＤＡＲ１１から取得したポイントクラウドに対して、カメラから取得した画像を補完的に用いることで、強度画像のフレームを生成してもよい。
【産業上の利用可能性】
【0095】
本発明は、ＬｉＤＡＲが搭載された車両で得られたデータから位置精度の高い３次元マップを生成する３次元マップ生成装置として、例えば、自律走行車両に用いられる３次元マップを生成する装置として、利用できる。
【符号の説明】
【0096】
１０ ３次元マップ生成装置
１１ ＬｉＤＡＲ
１２ ＧＰＳ
１３ ３次元データ生成部
１４ マップ生成部
１４ａ ＸＹ面誤差削減部
１４ｂ Ｚ面誤差削減部
１５ ３次元マップ
２０ 車両
１１ ＬｉＤＡＲ
１２ ＧＰＳ
１３ ３次元データ生成部
１４ マップ生成部
１４ａ ＸＹ面誤差削減部
１４ｂ Ｚ面誤差削減部
１５ ３次元マップ
２０ 車両
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15A】
【図15B】
【図16】
【図17】
【図18】
【図19】
【図20】
