特開 2024-17652 自己位置推定装置、自己位置推定方法及び自己位置推定プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024017652

(43)【公開日】2024-02-08

(54)【発明の名称】自己位置推定装置、自己位置推定方法及び自己位置推定プログラム

(51)【国際特許分類】

   G06T 7/70 20170101AFI20240201BHJP

   G06T 7/38 20170101ALI20240201BHJP

【ＦＩ】

G06T7/70 Z

G06T7/38

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2022120446

(22)【出願日】2022-07-28

(71)【出願人】

【識別番号】523286071

【氏名又は名称】株式会社ＮＴＴデータ

(74)【代理人】

【識別番号】110001243

【氏名又は名称】弁理士法人谷・阿部特許事務所

(72)【発明者】

【氏名】原田 智彦

(72)【発明者】

【氏名】下條 ひなた

【テーマコード（参考）】

5L096

【Ｆターム（参考）】

5L096AA06

5L096BA04

5L096CA04

5L096DA02

5L096FA09

5L096FA12

5L096FA67

5L096FA69

5L096GA51

5L096HA05

5L096KA04

(57)【要約】

【課題】ロストの発生等によって推定精度が不十分な自己位置の推定結果をも利用することができ、自己位置の推定精度をより高めることができる自己位置推定装置を提供する。
【解決手段】複数のフレーム画像を含む第１のシーケンスデータ、少なくとも一部が先のフレーム画像において撮影された地物と同一の地物を撮影したフレーム画像を含む第２のシーケンスデータとを対応付けるシーケンスデータ対応付け部１０１と、一方のフレーム画像に撮像された地物を撮像した他方のフレーム画像を抽出するフレーム画像抽出部１０３と、フレーム画像抽出部１０３において抽出された一方のフレーム画像における地物の位置と、他方のフレーム画像における地物の位置とに基づいて、地物の位置並びに地物を撮像した際のカメラの位置及び地物に対する向きを推定する自己位置推定部１０７と、によって自己位置推定装置１を構成する。
【選択図】図５

【特許請求の範囲】

【請求項1】

複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける対応付部と、
前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する撮像画像抽出部と、
前記撮像画像抽出部において抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際のカメラの位置及び前記地物に対する向きを推定する自己位置推定部と、
を含む、自己位置推定装置。

【請求項2】

前記対応付け部は、前記第１の撮像画像及び前記第２の撮像画像において、前記地物の間の共通の位置にキーポイントを設定し、当該キーポイントを用いて前記第１のシーケンスデータと前記第２のシーケンスデータとを対応付ける、請求項１に記載の自己位置推定装置。

【請求項3】

前記キーポイントは、前記地物の外縁に基づいて設定される、請求項２に記載の自己位置推定装置。

【請求項4】

前記自己位置推定部は、バンドル調整によって行われ、バンドル調整のコスト関数は、前記キーポイントを用いたことによる誤差を表す項を含む、請求項２に記載の自己位置推定装置。

【請求項5】

前記対応付け部は、前記第１のシーケンスデータを使って取得された第１の推定データ及び前記第２のシーケンスデータを使って取得された第２の推定データを使って前記第１のシーケンスデータと前記第２のシーケンスデータとを対応付ける、請求項１に記載の自己位置推定方法。

【請求項6】

前記推定データは、前記第１の撮像画像または前記第２の撮像画像が撮像された位置を示す測位情報を含む、請求項５に記載の自己位置推定方法。

【請求項7】

複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける工程と、
前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する工程と、
抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際のカメラの位置及び前記地物に対する向きを推定する工程と、
を含む、自己位置推定方法。

【請求項8】

複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける機能と、
前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する機能と、
抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際の前記カメラの位置及び前記地物に対する向きを推定する機能と、
を、コンピュータに実行させる、自己位置推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己の位置を推定する自己位置推定装置、自己位置推定方法及び自己位置推定プログラムに関する。

【背景技術】

【0002】

車両や配送ロボットの自律運転には、数ｃｍ級の精度の３次元地図が必要である。このよう３次元の地図を作成する公知技術は、計測機材を積んだ車両を走行させて自己位置や地物の相対的な位置を計測することを行っていた。また、近年、ドローンやドライブレコーダに搭載される単眼カメラで撮影した動画（フレーム画像のセット）を使ってＶｉｓｕａｌ ＳＬＡＭ（Simultaneous Localization And Mapping）を行い、比較的低コストに自己位置を推定してマップを作成する技術が検討されている。また、自己位置を計測する公知の方法としては、ＧＰＳ（Global Positioning System）を使った衛星測位がある。ただし、衛星測位は、特に都市部においてビルの谷間（アーバンキャニオン）で衛星の電波が反射、屈折されることで測位精度が低下することが知られている。ドローンやドライブレコーダを使った自己位置の推定は、ＧＰＳによって取得された情報を一部利用し、低コストながら十分な精度の測位を実現することができる。

【0003】

上記のようなＶｉｓｕａｌ ＳＬＡＭは、例えば、特許文献１、特許文献２に記載されている。特許文献１には、移動するカメラが車両周辺の画像を逐次撮像し、この画像に基づいてマップ情報を作成することが記載されている。また、特許文献２には、Ｖｉｓｕａｌ ＳＬＡＭによって推定された自己の位置姿勢を、例えばＡＭＣＬ（Adaptive Monte Carlo Localization）等の他のアルゴリズムで推定、または特定した自己位置を使って補正することが記載されている。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１８－１７３８８２号公報

【特許文献2】特開２０２０－１６０５９４号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

上記した特許文献１に記載のＶｉｓｕａｌ ＳＬＡＭ（代表的なものにＯＲＢ（Oriented FAST and Rotated BREIF） ＳＬＡＭがある）による自己位置推定は、複数のフレーム画像中の同一の特徴点を追跡する、トラッキング処理が必要である。しかしながら、トラッキングにおいては、しばしば特徴点の追跡に失敗する「ロスト」が発生する。ロストが発生すると、ロストが発生する以前のデータのみしか得ることができない。このようなデータを使って推定される自己位置は、取得されたデータの量が不十分であり、位置の推定精度が低下する。また、ロストの発生は、ロストが発生するまでに特徴点の追跡にかかった時間やコストをも無駄になることが課題になる。

【0006】

本発明は、上記の点に鑑みてなされたものであり、ロストの発生等によって推定精度が不十分な自己位置の推定結果をも利用することができ、自己位置の推定精度をより高めることができる、自己位置推定装置、自己位置推定方法及び自己位置推定プログラムに係る。

【課題を解決するための手段】

【0007】

上記目的を達成するために本発明の一形態の自己位置推定装置は、複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける対応付部と、前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する撮像画像抽出部と、前記撮像画像抽出部において抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際の前記カメラの位置及び前記地物に対する向きを推定する自己位置推定部と、を含む。

【0008】

本発明の一形態の自己位置推定方法は、複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータと、を対応付ける工程と、前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する工程と、抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際の前記カメラの位置及び前記地物に対する向きを推定する工程と、を含む。

【0009】

本発明の一形態の自己位置推定プログラムは、複数の第１の撮像画像を含む第１のシーケンスデータと、少なくとも一部が前記第１の撮像画像において撮影された地物と同一の前記地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける機能と、前記第１の撮像画像に撮像された前記地物を撮像した前記第２の撮像画像を抽出する機能と、抽出された前記第１の撮像画像における前記地物の位置と、前記第２の撮像画像における前記地物の位置とに基づいて、前記地物の位置並びに前記地物を撮像した際の前記カメラの位置及び前記地物に対する向きを推定する機能と、を、コンピュータに実行させる。

【発明の効果】

【0010】

以上の発明の態様によれば、ロストの発生等によって推定精度が不十分な自己位置の推定結果をも利用することができ、自己位置の推定精度をより高めることができる、自己位置推定装置、自己位置推定方法及び自己位置推定プログラムを提供することができる。

【図面の簡単な説明】

【0011】

【図1】本発明の一実施形態のフレーム画像における特徴点を説明するための図である。

【図2】（ａ）、（ｂ）、（ｃ）は、ＯＲＢを物体としての信号機及び標識に適用して特徴点を抽出することを説明するための模式図である。

【図3】（ａ）、（ｂ）、（ｃ）は、特徴点の位置の推定を説明するための図である。

【図4】本発明の一実施形態の自己位置推定方法の概念を説明するための図である。

【図5】本発明の一実施形態の自己位置推定装置を説明するための機能ブロック図である。

【図6】図５に示した自己位置推定装置によって行われる自己位置推定方法を説明するためのフローチャートである。

【図7】（ａ）から（ｄ）は、キーポイントを説明するための図である。

【発明を実施するための形態】

【0012】

以下、本発明の一実施形態の自己位置推定方法、この自己位置推定方法をコンピュータ上で実行する自己位置推定プログラム及び、自己位置推定方法を実行する自己位置推定装置を説明する。本実施形態においては、ＯＲＢ（Oriented FAST and Rotated BRIEF）－ＳＬＡＭを使って屋外の地図（以下、「マップ」と記す）を作成する例を挙げて説明する。また、本実施形態は、例えば車両やドローン等の移動体に単眼カメラを搭載し、このカメラを使って撮像された動画を連続する複数のフレーム画像として取得する例を説明する。本明細書でいう「自己位置」は、カメラの撮像位置であり、撮像位置は、２次元、または３次元で示される座標で表される位置をいい、撮像角度はカメラの被写体に向かう高さ方向及び平面方向の角度をも含む。また、本明細書では、カメラのうちのレンズが被写体に向かう角度を「姿勢」とも記す。なお、角度は、例えば、レンズの中心点を通る水平な中心軸と、レンズの中心点と被写体の基準点（例えば中心）とを通る軸とがなす角度により規定してもよい。

【0013】

また、本実施形態は、上記のようにして行った一連のフレーム画像によって構成されるデータの各々を、以降「シーケンスデータ」と記す。シーケンスデータに含まれるフレーム画像の数に規定はない。また、作成されたシーケンスデータのトラッキング処理において特徴点のロストが発生した場合、ロストが発生した以降の処理は行われない。本発明の発明者らは、このようなフレーム画像の少ないシーケンスデータによって行われた自己位置の推測結果を組み合わせ、より高い精度で自己位置を推定したマップを作成することに着目し、本発明に想到した。

【0014】

本実施形態においては、先ず、シーケンスデータによる基本的な自己位置の推定について説明し、この後にシーケンスデータの自己位置の推定の結果を組み合わせることについて説明する。

【0015】

［シ―ケンスデータを使った自己位置の推定］
（特徴点の抽出）
図１は、フレーム画像における特徴点ｐを説明するための図である。本実施形態において、特徴点ｐの抽出は、ＯＲＢの手法によって行われる。特徴点は、連続するフレーム画像において位置が大きく変化せず、抽出し易いものが選択される。図１に示す例は、窓やドアの四角部分、あるいは角部分のコーナーを特徴点として抽出したフレーム画像を示している。

【0016】

図２は、ＯＲＢを物体としての信号機及び標識に適用して特徴点を抽出することを説明するための模式図である。本実施形態は、車両やドローンに搭載されたカメラが移動しながら信号機１００、３００及び標識２００を撮像する。このため、図２に示すように、位置の異なる複数のカメラによって信号機１００及び標識２００が撮像される場合と同様の画像を得ることができる。図２においては、異なる３つの撮像位置にある例えば１つのカメラによって撮像される３枚のフレーム画像を、それぞれｆ_N-1、ｆ_N、ｆ_N+1とする。図２に示す例では、フレーム画像ｆ_N-1、ｆ_Nには信号機１００と標識２００とが映り込み、フレーム画像ｆ_N+1には標識２００と信号機３００とが映り込んでいる。なお、以降の説明において、フレーム画像ｆ_N-1、ｆ_N、ｆ_N+1を区別する必要がない場合、単にフレーム画像ｆと記す。

【0017】

本実施形態は、フレーム画像ｆ_N-1において、映り込んだ信号機１００の複数のコーナーＣ１００を特徴点ｐ_N-1,1として、標識２００の複数のコーナーＣ２００を特徴点ｐ_N-1,2としてそれぞれ抽出する。また、フレーム画像ｆ_Nから信号機１００のコーナーＣ１００を特徴点ｐ_N,1として、標識２００のコーナーＣ２００を特徴点ｐ_N,2として抽出し、フレーム画像ｆ_N+1から標識２００のコーナーＣ２００を特徴点ｐ_N+1,1として、標識２００のコーナーＣ２００を特徴点ｐ_N+1,2として抽出する。ＯＲＢによる特徴点の抽出は、ＦＡＳＴ（Features from Accelerated Segment Test）とＢＲＩＦ（Binary Robust Independent Elementary Features）とを組み合わせて行われる。このような処理によれば、フレーム画像の注目点の周囲の複数の点（例えば１６点）のうち、注目点より閾値以上に明るい点が所定の数（例えば１２点）以上存在する場合、注目点を特徴点に採用する。さらに、ＯＲＢでは、採用された特徴点の周辺領域からランダムにピクセルペアをとり、その輝度に応じて０、または１を決定する処理を行っている。

【0018】

（特徴点位置の推定）
図３（ａ）、図３（ｂ）及び図３（ｃ）は、抽出された特徴点の位置の推定を説明するための図である。図３（ａ）から図３（ｃ）においては、１つのカメラの異なる撮像位置の３つをそれぞれカメラＮ－１、Ｎ、Ｎ＋１と記し、各撮像位置において撮像されるフレーム画像を、それぞれフレーム画像ｆ_N-1、フレーム画像ｆ_N、フレーム画像ｆ_N+1と記す。特徴点の位置の推定は、先ず、以前に行ったマップ作成処理において位置を特定した特徴点を特定する。既知のマップは相対的なＳＬＡＭ座標を使って表される。

【0019】

次に、本実施形態は、既知のマップ中の特徴点（以下、「マップポイント」と記す）の位置を基準にし、フレーム画像ｆ_N-1を撮像したカメラＮ－１、カメラＮの姿勢及び自己位置を推定する。自己位置の推定により、カメラＮ－１_、カメラＮの自己位置は、既知の位置となる。特徴点位置の推定は、カメラＮ－１、カメラＮの自己位置に基づいて、位置が既知でない（未知である）特徴点の位置を推定する（Local Mapping スレッド）。次に、本実施形態は、推定によって位置が得られた特徴点を既知のマップポイントとし、マップポイントの位置に基づいてカメラＮ＋１の姿勢及び位置、すなわち、自己位置を推定する）（Tracking スレッド）。このように、特徴点の位置の推定とカメラの姿勢及び自己位置の推定は、交互に行われる。

【0020】

なお、上記処理は、Ｖｉｓｕａｌ ＳＬＡＭにおいては基本的にカメラの内部のパラメータが既知であることによって実現できる。カメラの３次元の位置は、ＲＡＮＳＡＣ(Random Sample Consensus)を使って誤対応を除去しながら行われる。例えば、特徴点の３次元座標は、この特徴点を写す例えば二つのカメラＮ－１、カメラＮの姿勢及び自己位置により形成される三角測量によって計算できる。

【0021】

（自己位置の補正（バンドル調整））
さらに、自己位置の補正は、バンドル調整によって行われる。バンドル調整は、撮像されたフレーム画像のうちのキーフレーム画像から推定される自己の位置を補正することによって行われる。ここで、「キーフレーム画像」は、フレーム画像のうちのカメラポジションの補正に使用されるものを指す。補正は、キーフレーム画像内にマップポイントが実際に撮像された位置と、本来撮像されるであろう再投影位置との差分を算出し、算出された差分である再投影誤差が減少するように、カメラの位置及び角度に係る撮像情報を補正（最適化）することによって行われる。撮像情報は、少なくともカメラの角度及び位置に関する情報を含んでいればよく、適宜他の情報を含んでいてもよい。なお、再投影位置は、キーフレーム画像にマップポイントの実位置を投影することによって決定される。再投影位置の補正は、ＳＬＡＭ空間において行われる。

【0022】

式（１）は、本実施形態の自己位置の補正に用いられる演算式であって、バンドル調整を採用している。式（１）は、カメラの位置ｔ、カメラの姿勢ｒ、マップポイント（点群）の位置ｐのコスト関数である。カメラの位置ｔは並進座標系で表され、カメラの姿勢ｒは回転座標系で表される。式（１）中のＦはフレーム画像の集合、Ｓ_iはフレーム画像ｉ上の特徴点の集合を示し、ｑ_ijはフレーム画像ｉ上の特徴点ｊの座標を示し、proj（ti,ri,pj）は、マップポイントpjをカメラポーズti,riを用いてキーフレーム画像上に再投影した座標を示している。

【0023】

【数1】

・・・式（１）

【0024】

次に、ＳＬＡＭ空間のマップは、ＧＰＳ座標で表される実空間へマッピングされる。ＳＬＡＭ座標系とＧＰＳ座標系は、それぞれ直交座標系であり、相似変換（Similarity Transformation）によって互いに変換可能である。なお、変換に用いられるパラメータは、例えば、公知の手法であるｕｍｅｙａｍａ法を用いて推定することができる。ｕｍｅｙａｍａ法については、例えば、S-Umeyama(1991).”Least-squares estimation of transformation parameters between two point patterns”, Umeyama, (1991),IEEE volume13,Page(s):376-380に開示されている。本実施形態の各シーケンスデータにおいて行われる自己位置の推定処理は以上である。

【0025】

次に、本実施形態は、以上説明した自己位置推定の結果を用い、より精度が高い自己位置の推定を行う。図４は、本実施形態の自己位置推定方法の概念を説明するための図であって、フレーム画像ｆ１からｆｎを含むシーケンスデータと、フレーム画像ｆ１からｆｍを含むシーケンスデータと、を示している。ここで、「シーケンスデータ」は、撮影開始から終了までの一連の連続する画像フレームからなる動画を指す。本実施形態では、以降、フレーム画像ｆ１からｆｎを含むシーケンスデータをシーケンスデータＮ、フレーム画像ｆ１からｆｍを含むシーケンスデータをシーケンスデータＭとする。シーケンスデータＮ、Ｍは、いずれも信号機４００に設定された特徴点（既知である場合にはマップポイント）を追跡し、追跡の過程で特徴点のロストが発生している。ロストの発生により、以降のフレーム画像Ｌｎ、Ｌｍによる自己位置推定の処理は中止される。

【0026】

本実施形態は、シーケンスデータＭ、Ｎからフレーム画像Ｌｎ、Ｌｍを除いたフレーム画像にキーポイントｐ_kを設定し、フレーム画像を撮像したカメラの位置や姿を高い精度で推定するものである。ただし、本実施形態は、ロストが発生したシーケンスデータ同士を適用する例を挙げて説明するが、本実施形態は、このような例に限定されるものでない。すなわち、本実施形態は、２つのシーケンスデータを用いることに限定されず、複数であれば任意の数のシーケンスデータを集約して行うことができる。さらに、複数のシーケンスデータがロストを発生したシーケンスデータを含むか否かは任意である。

【0027】

本実施形態は、シーケンスデータの作成、すなわち撮影後に、自己位置の推定処理を実行する例を挙げて説明する。本実施形態の自己位置の推定は、先に説明した自己位置の推定のトラッキング処理においてロストが発生した以降のタイミングで開始される。

【0028】

図５は、本実施形態の自己位置推定装置を説明するための機能ブロック図である。図６は、図５に示した自己位置推定装置１によって行われる自己位置推定方法を説明するためのフローチャートである。図５に示すように、自己位置推定装置１は、シーケンスデータ対応付け部１０１、フレーム画像抽出部１０３、キーポイント設定部１０５及び自己位置推定部１０７を含んでいる。

【0029】

シーケンスデータ対応付け部１０１は、撮像された複数の第１の撮像画像（フレーム画像）を含む第１のシーケンスデータと、少なくとも一部が第１の撮像画像において撮影された地物と同一の地物を撮影した第２の撮像画像を含む第２のシーケンスデータとを対応付ける。本実施形態は、シーケンスデータＭが第１のシーケンスデータに相当し、シーケンスデータＮが第２のシーケンスデータに相当する。フレーム画像抽出部１０３は、撮像画像抽出部に相当し、シーケンスデータＭに含まれるフレーム画像に撮像された地物を撮像したシーケンスデータＮに含まれるフレーム画像を抽出する。本実施形態でいう「地物」は、地上に存在する自然、人工物を含む。自然物は、例えば、山や樹木であってもよいし、人工物は建物の他、信号機や標識といった緯度、経度が既知の公的基準点であってもよい。本実施形態の処理に使用される地物を、以降「検出物体」と記す。

【0030】

自己位置推定部１０７は、バンドル調整を用いてカメラ、またはフレーム画像の位置及び姿勢を繰り返し推定する。本実施形態では、自己位置推定部１０７において行われるバンドル調整を「後付けバンドル調整」ともいう。後述するように、後付けバンドル調整の処理は、先に説明したバンドル調整と基本的には同様に行われる。さらに、自己位置推定装置１は、キーポイント設定部１０５を備えている。

【0031】

本実施形態においては、先に説明したＯＲＢ－ＳＬＡＭの実行中に特徴点のロストが発生すると、その時点で保持している推定データを全て所定のファイルに出力する（ステップＳ１０１）。推定データは、マップポイントとなる既知の特徴点のフレーム画像上での位置（２次元座標）、マップポイントの位置（３次元座標）、及びマップポイントが撮影されているフレーム画像（カメラ）の位置及び姿勢を含む。なお、ロスト発生直後に出力される座標は、マッチポイントやフレーム画像（カメラ）の３次元の位置を示すＳＬＡＭ座標である。このため、推定データの座標は、例えば先に述べた公知のｕｍｅｙａｍａ法を用いてＧＰＳ座標に変換される。

【0032】

シーケンスデータ対応付け部１０１は、撮像と同時に取得されたＧＰＳ座標を検索する（ステップＳ１０２）。なお、ＧＰＳ情報は、緯度や経度を使ったＧＰＳ座標で表される地物の座標をいう。ただし、測位情報は、緯度や経度を用いない平面直角座標であってもよい。また、ＧＰＳ座標は、シーケンスデータに基づいて作成された自己位置の推定データに含まれる、各キーフレーム画像（カメラ）のＧＰＳ座標であってもよい。

【0033】

そして、シーケンスデータ対応付け部１０１は、ＧＰＳ座標が示す位置が一致する、または近いシーケンスデータをグループ化する。本実施形態では、シーケンスデータＭとシーケンスデータＮとが対（ペア）をなすようにグループ化されるものとする（ステップＳ１０３）。このような処理によれば、ペアとなるシーケンスデータＭ、Ｎには同じ検出物体が撮影されている可能性が高くなる。なお、ＧＰＳ座標が示す位置が近いとは、例えば、一方のシーケンスデータのＧＰＳ座標が示す位置を基準にし、他方のシーケンスデータのＧＰＳ座標が示す位置との相違が予め設定されている範囲内に含まれる場合を指すようにしてもよい。

【0034】

次に、フレーム画像抽出部１０３は、グループ化されたシーケンスデータＭ、Ｎのフレーム画像に同じ検出物体が映っているか否かを判定する。シーケンスデータのＧＰＳ座標に基づく位置が近い場合であっても、フレーム画像（カメラ）の姿勢によっては同じ検出物体が写っていない場合があるからである。検出物体がフレーム画像に写っているか否かの判定は、例えば、機械学習を用いた物体検出アルゴリズムによって行ってもよい。また、自己位置推定装置の操作者が目視によって行ってもよい。なお、１つのフレームに複数の検出物体が写っている場合、これを区別するために、例えば、ハンガリアン法を用いて、マッチング処理を行ってもよい。ただし、本実施形態は、このような構成に限定されるものでなく、任意の手法によって検出物体とランドマークとをマッチングするようにしてもよい。

【0035】

次に、キーポイント設定部１０５は、同じ検出物体が写るフレーム画像上にキーポイントを設定する（ステップＳ１０４）。キーポイントの設定は、ペアをなすシーケンスデータＭ、Ｎのうちから同じ検出物体が写るフレーム画像を複数選択し、同一の検出物体における同一の箇所の位置を２次元座標により求めることによって行われる。

【0036】

図７（ａ）から図７（ｄ）は、キーポイントｐ_kを説明するための図である。図７（ａ）から図７（ｄ）に示す例は、検出物体を信号機４００とし、信号機４００の矩形形状の輪郭の図中の右下頂点の２次元座標をキーポイントｐ_kに設定している。なお、キーポイントｐ_kは、このように設定することに限定されず、同一の検出物体における同一の位置を示す点であればよい。キーポイントｐ_kは、信号機４００（地物）の外縁に基づいて設定されるようにしてもよい。外縁は、信号機の４つの頂点と、頂点との間を接続する直線によって表される。外縁に基づくキーポイントｐ_kは、例えば、４つの頂点のいずれか１つ、あるいは頂点を結ぶ信号機４００の底辺上の点（例えば中央の点）、さらには、対角をなす頂点同士を結ぶ直線が交差する信号機４００の中心点であってもよい。

【0037】

図７（ａ）は、フレーム画像中に信号機４００がカメラに対して正対するように写っている例を示す。このような場合、信号機４００の全体がフレーム画像に示されているから、信号機４００の輪郭の右下頂点の位置が明らかである。キーポイント設定部１０５は、右下頂点をキーポイントｐ_kに設定する。図７（ｂ）は、フレーム画像中に信号機４００がカメラに対して正対するように写っていて、フレーム画像における信号機４００の位置が図７（ａ）に比べて下方にある例を示す。図７（ｂ）においても、信号機４００の全体がフレーム画像に写っていて、右下頂点の位置が明らかである。このため、キーポイント設定部１０５は、図７（ｂ）に示す信号機４００の右下頂点をキーポイントｐ_kに設定する。図７（ｃ）は、信号機４００がカメラに対して斜めを向いた状態で写っている例を示している。図７（ｃ）の場合であっても、フレーム画像から信号機４００の右下頂点の位置は明らかであるが、奥行きのある画像の頂点をキーポイントＰ_kとして検出することは、奥行きのない画像からキーポイントＰ_kを検出するよりも困難であり、処理の精度を低下させる虞がある。このため、キーポイント設定部１０５は、検出物体のフレーム画像の面と交差する方向の傾きを検出し、傾きが許容される程度より大きい場合、このフレーム画像をキーポイントＰｋの設定の対象から除くようにしてもよい。

【0038】

図７（ｄ）は、フレーム画像に信号機４００の一部のみが写っている例を示している。本実施形態は、信号機４００の右下頂点をキーポイントｐ_kに設定しており、右下頂点はフレーム画像に写っているためにキーポイントｐ_kの設定が可能である。ただし、キーポイントｐ_kを信号機４００の下辺、あるいは全体の中心点に設定した場合、信号機４００の一部が写ったフレーム画像からは中心点の判断ができないため、キーポイントｐ_kを設定することができない。

【0039】

次に、自己位置推定部１０７は、以上の結果を用いて後付けバンドル調整を行う（ステップＳ１０５）。後付けバンドル調整は、先に説明したバンドル調整と基本的に同様の処理によって行われる。すなわち、後付けバンドル調整にあっても、複数のマップポイントの位置（座標）と、マップポイントが写ったキーフレーム画像(カメラ)の位置及び姿勢（以下、「カメラポーズ」と記す）が既知であれば、マップポイントの位置とカメラポーズとを最適化することが可能である。最適化は、キーフレーム画像のカメラポーズによってマップポイントの３次元で表される位置をフレーム画像に投影して２次元座標を得る処理を含む。そして、得られた２次元座標と実際にフレーム画像に写るマップポイントの２次元座標との差を最小化するコスト関数を拘束条件としたグラフ最適化問題として定式化し、式をＧ２ＯライブラリやＣｅｒｅｓ Ｓｏｌｖｅｒライブラリ、ＧＴＳＡＭライブラリ等を用いて解くことによって行われる。

【0040】

後付けバンドル調整において使用されるコスト関数は、式（１）に示した基本的な１項のコスト関数に加え、キーポイントを用いたことによる誤差項を含む二項のコスト関数になる。このようなコスト関数を、式（２）に示す。

【0041】

【数2】

・・・式（２）
式（２）中の記号は以下のとおりである。
V ：シーケンスデータの集合
F_k ：シーケンスデータｋに含まれるフレーム画像の集合
Ｓ_i：フレーム画像ｉに映る特徴点の集合
Ｋ_i：フレーム画像ｉに映るキーポイントの集合
ｑ_ij：特徴点ｊのフレーム画像ｉにおける座標
ｑ_ik：キーポイントｋのフレーム画像ｉにおける座標
ｐｒｏｊ（ｔ_i，ｒ_i，ｐ_j）：特徴点ｊをカメラポーズｔ_i，ｒ_iを用いてフレーム画像に再投影した座標
ｐｒｏｊ（ｔ_i，ｒ_i，ｐ_k）：キーポイントｋ（明細書中ではｐ_k）をカメラポーズｔ_i，ｒ_iを用いてフレーム画像に再投影した座標
ｗ₁，ｗ₂：各項の重みの調整係数

【0042】

上記したように、シーケンスデータＭ、Ｎをグループ化する場合、各シーケンスデータのマップポイントとキーフレーム画像とを使ってそれぞれグラフを作成することができる。ここで、グラフとは、Ｇ２Ｏライブラリ等のアルゴリズムを用いる場合に使用されるグラフをいう。Ｇ２Ｏライブラリ等のアルゴリズムにおいては、マップポイントとキーフレーム画像をノードとしマップポイントを映し出すキーフレーム画像との間にエッジ（拘束条件）を張ったグラフ構造を作って処理を行う。シーケンスデータＭ、Ｎのグラフをつなぐエッジとして、本実施形態は、キーポイントを設定する。キーポイントの設定により、本実施形態は、シーケンスデータＭ、Ｎの各々について作成されたグラフを１つのグラフとして扱い、後付けバンドル調整を行っている。以下、後付けバンドル調整の手順を説明する。

【0043】

（１）シーケンスデータの座標系を揃える
グループ化されたシーケンスデータの推定データは、シーケンスデータごとに異なるＳＬＡＭ座標系で表現されている。複数のシーケンスデータをまとめて扱うためには、それぞれのＳＬＡＭ座標系を、例えば公知のＵｍｅｙａｍａ法によってＧＰＳ座標に変換し、座標系を統一する。

【0044】

（２）キーポイントの３次元による位置を求める
（２-１）検出物体の緯度、経度が不明である場合
ペアをなすシーケンスデータのうち、一方のシーケンスデータの複数のフレーム画像に写るキーポイントの２次元の位置及びカメラポーズを用い、三角測量によってキーポイントの３次元位置を求める。同様の、他方のシーケンスデータにおいてもキーポイントの３次元位置を求める。それぞれの３次元位置が一致することが望ましいが、一致しない場合は両者の重心を３次元位置に代えて使用する。

【0045】

（２-２）検出物体の緯度、経度が既知である場合
検出物体が信号機等の公共基準点等、予め緯度、経度が既知な場合、既知の緯度、経度が示す３次元的な位置を使用する。

【0046】

（３）２つのグラフを作成する
ペアをなすシーケンスデータの推定データについて、それぞれマップポイントとキーフレーム画像とをノードとし、マップポイントを写すキーフレーム画像との間にエッジを張ったグラフで表現し、エッジにはキーフレーム画像のカメラポーズによりマップポイントの３次元位置をフレーム画像上の２次元位置に投影し、実際にフレーム画像に写るマップポイントの２次元位置との差を最小化するコスト関数を各エッジの拘束条件に設定する。このような処理は、公知のＶｉｓｕａｌ ＳＬＡＭにおけるバンドル調整によって行うことができる。

【0047】

（４）２つのグラフを接続する
（３）で作成されたグラフを、２－１、２－２で求めたキーポイントの３次元位置をノードとして追加し、キーポイントと、このキーポイントが写るキーフレーム画像との間にエッジを追加する。エッジ（拘束条件）は、キーフレーム画像のカメラポーズによってキーポイントの３次元位置をフレーム画像上の２次元位置に投影し、実際にフレーム画像に写るキーポイントの２次元位置との差を最小化するコスト関数として設定される。

【0048】

（５）後付けバンドル調整
接続したグラフ全体において、グラフ最適化問題を解ことによって後付けバンドル調整（ｒｅｔｒｏｆｉｔ ＢＡ）を実行する。最適化は、Ｖｉｓｕａｌ ＳＬＡＭと同様に、Ｇ２ＯライブラリやＣｅｒｅｓ Ｓｏｌｖｅｒライブラリ、ＧＴＳＡＭライブラリ等を用いて最適化問題を解くことによって実現できる。

【0049】

以上説明したように、本実施形態は、複数のシーケンスデータをキーポイントを設定することによって対応付けて自己位置を推定することができる。このような本実施形態は、シーケンスデータの推定データ数が少なくても多くの回数マップポイントの位置とカメラポーズを補正し、自己位置の推定精度を高めることができる。本実施形態は、例えば、特徴点のロストが発生したデータを自己位置の推定に活かすことができ、処理効率を高めることができる。

【符号の説明】

【0050】

１ 自己位置推定装置
１００，３００，４００ 信号機
１０１ シーケンスデータ対応付け部
１０３ フレーム画像抽出部
１０５ キーポイント設定部
１０７ 自己位置推定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】