JP2023106163A - 自己位置推定方法、自己位置推定プログラム及び自己位置推定装置 - Google Patents

Abstract

【課題】自己位置の推定の精度がより高い自己位置推定方法、自己位置推定プログラム及びこの自己位置推定プログラムを使って自己位置を推定する自己位置推定装置を提供する。【解決手段】方法は、複数のフレーム画像に基づいて設定されるＳＬＡＭ空間において検出され、位置が推定された検出物体を、ＳＬＡＭ空間の座標と異なる他のＧＰＳ座標によって規定される実空間において位置が既知であるランドマークと対応付ける工程Ｓ３０１と、ランドマークに対応付けられた検出物体のフレーム画像において撮像されるはずの位置である再投影位置を決定する工程Ｓ３０２と、再投影位置と、フレーム画像におけるランドマークに対応付けられた検出物体の実際の位置との差分を最小化することにより、少なくともフレーム画像の撮像位置及び撮像角度を含む撮像情報を推定する工程Ｓ３０３とＳ３０４、により自己位置を推定する。【選択図】図１０

Description

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

移動しながら画像を撮像し、この画像に基づいて移動体周辺の三次元空間の環境（空間の形状や障害物に関する情報）を推定する技術がマップの作成やロボットの自動走行等に使用されている。このような技術には、例えば、ＳｆＭ（Structure from Motion）やｖ－ＳＬＡＭ（visual-Simultaneous Localization And Mapping）がある。ＳｆＭとｖ－ＳＬＡＭとは、多くの場合、実時間性を前提とするか否かによって相違し、ＳｆＭは、撮像された順番に依らない複数の画像を処理することに多く適用され、ｖ－ＳＬＡＭは、動画として撮像された複数のフレーム画像を処理することに多く適用される。このような技術は、例えば、特許文献１、特許文献２に記載されている。

特許文献１には、移動するカメラが車両周辺の画像を逐次撮像し、この画像に基づいてマップ情報を作成することが記載されている。マップ情報とは、キーフレームテーブルとマップ点テーブルとで表現され、マップ点は特徴点の三次元座標をいう。特徴点は、対象領域に存在する物体の形状を表す画像内のエッジ点等をいう。特許文献１に記載の情報処理装置等は、撮像されたグレースケール画像に投影されたマップ点と、特徴点との距離が最小にするようなカメラの位置及び姿勢を、非線形最適化アルゴリズムを使って算出し、カメラの位置姿勢を推定する。また、特許文献２には、ｖ－ＳＬＡＭによって推定された自己の位置姿勢を、例えばＡＭＣＬ（Adaptive Monte Carlo Localization）等の他のアルゴリズムで推定、または特定した自己位置を使って補正することが記載されている。

特開２０１８－１７３８８２号公報 特開２０２０－１６０５９４号公報

しかしながら、上記した特許文献１に記載のｖ－ＳＬＡＭによる自己位置推定は、最初に２視点から撮像されたグレースケール画像間で特徴点の対応付けを行う。そして、１視点目に対応するカメラの位置及び姿勢を世界（グローバル）座標の原点に設定し、２視点目に対応するカメラの位置及び姿勢の初期値を設定し、三角測量によりカメラの位置及び姿勢の初期値を設定する。特許文献１に記載の情報処理装置は、このようにして、車両の移動に合わせてカメラが移動する際に、マップ点を用いてカメラの位置及び姿勢を逐次推定している。特許文献１に記載の手順は、車両を基準にしたローカル座標をグローバル座標の原点に設定しているため、原点にずれが生じた場合、このずれが推定されたカメラの位置及び姿勢に影響し、推定の精度を高めることに不利である。また、特許文献２に記載のＡＭＣＬは、測距データと移動環境のマップデータとを使って自己位置を推定し、マップデータに二次元の格子マップを用いている。このような手法は、ｖ－ＳＬＡＭによる自己位置推定と同様に、相対的な基準によって自己位置を推定するため、のｖ－ＳＬＡＭによる自己位置推定の精度を高めるには不十分であると考えられる。

本発明は、上記の点に鑑みてなされたものであり、自己位置の推定の精度がより高い自己位置推定方法、自己位置推定プログラム及び自己位置推定装置に関する。

上記目的を達成するために本発明の一形態の自己位置推定方法は、撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け工程と、前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定工程と、前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定工程と、を含む。

本発明の自己位置推定プログラムは、撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け機能と、前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定機能と、前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定機能と、を、コンピュータに実行させる。

本発明の一形態の自己位置推定装置は、撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け部と、前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定部と、前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定部と、を含む。

以上の発明の態様によれば、自己位置の推定の精度がより高い自己位置推定方法、自己位置推定プログラム及び自己位置推定装置を提供することができる。

本発明の一実施形態の自己位置推定装置を説明するための機能ブロック図である。 フレーム画像における特徴点を説明するための図である。 本発明の一実施形態のOＲＢを検出物体としての信号機及び標識に適用して特徴点を抽出することを説明するための模式図である。 （ａ）、（ｂ）（ｃ）は、図３の処理により抽出された特徴点の位置推定について説明するための図である。 本発明の一実施形態のマッチング処理において行われる、測位情報のベイシス対の作成について説明するための図である。 図５で説明したベイシス対を共通のハッシュテーブルにマッピングした例を示す図である。 特徴点の群を図５、図６で説明した処理と同様に処理し、共通のハッシュテーブルにマッピングすることを説明するための図である。 図１の機能ブロック図の演算処理部を処理のスレッド毎に表現した図である。 自己位置推定装置において実行される処理を説明するためのフローチャートである。 自己位置推定装置において実行される、図９に続く処理を説明するためのフローチャートである。 図１０のフローチャートにおいて、マッチング部によって行われる処理の一連の流れを説明するためのフローチャートである。

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

［自己位置推定装置］
図１は、本実施形態の自己位置推定装置１を説明するための機能ブロック図である。図１は、自己位置推定装置１の他、ランドマークデータベース（ＤＢ）５５、フレーム画像データベース（ＤＢ）５６、検出物体データベース（ＤＢ）５７を含むシステムを示している。ランドマークＤＢ５５は、マップ作成の対象となる環境のランドマークとなる地物の精度の高い測位情報を保存する。測位情報は、測位によって得られたものであればよく、緯度や経度を使ったＧＰＳ座標で表される地物の座標であってもよいし、緯度や経度を用いない平面直角座標であってもよい。また、本実施形態は、測位情報の他、ランドマークの種別や名称といった属性をランドマークＤＢ５５に保存する。さらに、図１に示すシステムは、作成されたマップを保存するマップデータベース（ＤＢ）５９を備えている。

フレーム画像ＤＢ５６は、車両等に搭載された図示しないカメラによって撮像された動画から抽出された複数の画像（以下、「フレーム画像」と記す）を保存する。フレーム画像の抽出は、例えば、動画を一定の時間ごとに切り出して複数の静止画像を生成することによって行うことができる。動画を撮像するカメラとしては、単眼カメラ、ＲＧＢ－Ｄカメラ、ステレオカメラが使用される。検出物体ＤＢ５７は、予めフレーム画像から検出された物体のフレーム画像上での検出ボックスの範囲を示す二次元座標を保存する。検出ボックスの範囲は矩形またはポリゴンで囲まれたものであればよく、画像解析によって検出されたものであってもよいし、機械学習による物体検出やセグメンテーションモデルで推定したものでもよい。本実施形態では、物体として、信号機や標識といった複数のフレーム画像中に共通して写り込むものが好ましい。物体の位置は、物体上の特徴点の位置により記録され、位置は、ＳＬＡＭ空間上で定義されるＳＬＡＭ座標、またはＧＰＳ座標によって規定される。さらに、本実施形態は、記録された物体によりカメラによって撮影された環境のマップを作成、あるいは補正（更新）する。作成、あるいは補正されたマップは、マップＤＢ５９に保存される。マップＤＢ５９に保存されたマップは、後述するように、順次撮影されたフレーム画像のうちのキーフレーム画像によって補正され、その精度が高められる。

図１に示すように、自己位置推定装置１は、演算処理部１０と、受信部２と、自己位置推定の処理の結果を出力する出力部３を有している。受信部２は、ＧＰＳ衛星Ｇから測位情報を受信するユーザセグメントである。受信部２は、特徴点のＳＬＡＭ座標をＧＰＳ座標に変換するためのＧＰＳ座標を取得することに使用される。出力部３は、ディスプレイ画面であってもよいし、結果を印字するプリンタ、あるいは音声出力するスピーカであってもよい。

演算処理部１０は、撮像によって生成された複数のフレーム画像から、フレーム画像の少なくとも一部に表示されている共通の複数の特徴点を順次抽出する抽出部を有している。ＯＲＢ－ＳＬＡＭを用いる本実施形態では、特徴点を抽出する特徴点抽出部１４が抽出部として機能する。また、演算処理部１０は、複数のフレーム画像に基づいて設定される第１の空間における特徴点の位置を推定する特徴点位置推定部１１と、特徴点位置推定部１１において位置が推定された特徴点を、第１の空間の座標と異なる他の座標によって規定される第２の空間における位置が既知であるランドマークと対応付けるマッチング部１２（対応付け部）と、を備えている。本実施形態では、第１の空間を相対的な座標により表されるＳＬＡＭ空間とし、この座標をＳＬＡＭ座標とする。また、第２の空間を測地系に基づく実空間とし、この座標をＧＰＳ座標とする。ただし、実空間は、環境の測位に基づいて規定される空間であればよく、ＧＰＳを使って測位された情報に基づくことに限定されるものではない。

また、演算処理部１０は、ランドマークの既知の位置に基づいて、ランドマークに対応付けられた特徴点が撮像画像において撮像されるはずの位置である再投影位置を決定する決定部、及び再投影位置と、ランドマークに対応付けられた特徴点のフレーム画像における実際の位置との差分を最小化することにより、少なくともフレーム画像の撮像位置及び撮像角度を含む撮像情報を推定する自己位置推定部１３を備えている。演算処理部１０は、例えば、汎用的なパーソナルコンピュータ上で自己位置推定のプログラムを実行することによって実現される。演算処理部１０の特徴点抽出部１４、特徴点位置推定部１１、マッチング部１２及び自己位置推定部１３の各構成は、プログラムと、このプログラムの実行に使用されるメモリ、ＣＰＵ（Central Processing Unit）及びユーザインターフェース等とによって実現される。

図１に示す自己位置推定装置１は、マップを作成するための構成の全てを含んでいる。自己位置推定装置１は、複数の構成に分割され、各構成が異なる場所にあって、それぞれの処理を行ってもよい。このような場合、自己推定装置１の構成はどのような組み合わせで分割されてもよく、また、自己推定位置１に含まれる図１に示す機能が複数あって、機能は分割された複数の構成の各々にあってもよい。また、本実施形態は、自己位置推定装置１の全体が複数の場所にあってそれぞれ処理を行ってもよい。このような構成は、例えば、複数の場所にある汎用的なコンピュータの各々で本実施形態の自己推定位置プログラムを動作させることによって簡易に実現することができる。本実施形態は、複数の場所にある自己推定装置１がそれぞれ機能し、マップ作成の全体の処理を分担して行う例を説明する。ただし、本実施形態は、このような構成に限定されるものでなく、図１に示すシステムが移動しながらフレーム画像を取得し、リアルタイムでマップを作成してもよい。

（特徴点抽出部）
図２は、フレーム画像における特徴点ｐを説明するための図である。本実施形態において、特徴点ｐの抽出は、例えば、ＯＲＢの手法によって行われる。特徴点は、連続するフレーム画像において位置が変化せず、抽出し易いものが選択される。図２に示す例は、窓やドアの四角部分、あるいは角部分のコーナーを特徴点として抽出したフレーム画像を示している。

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

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

以上の処理において、本実施形態では、複数の特徴点ｐ_N-1,1、ｐ_N-1,2がそれぞれ検出ボックスｂ１、ｂ２に内包される特徴点として認識される。また、同様に、フレーム画像ｆ_N+1の特徴点ｐ_N+1,3が検出ボックスｂ３に内包される特徴点と認識される。検出ボックスｂ１、ｂ２、ｂ３に内包される特徴点は、図１の検出物体ＤＢ５７に検出物体の座標の一部分として保存される。保存される座標はＳＬＡＭ座標である。なお、検出ボックスｂ１、ｂ２、ｂ３間の画像は物体の背景であると推定することができる。

また、本実施形態では、特徴点抽出部１４が、検出ボックスｂ１、ｂ２、ｂ３によって規定される信号機１００等の検出物体に対し、各物体を識別するためのＩＤ（IDentity）情報を付与する。ＩＤの付与は、複数のフレーム画像ｆに映り込んでいる物体に対して行われ、１つのフレーム画像ｆにのみ映り込んでいる物体に対しては行われない。つまり、図４(ａ)、図４（ｂ）、図４（ｃ）に示す例においては、２つのフレーム画像ｆ_N-1、ｆ_Nに映り込んでいる信号機１００、３つのフレーム画像ｆ_N-1、ｆ_N、ｆ_N+1に映り込んでいる標識２００に対してＩＤ情報が付与される。そして、フレーム画像ｆ_N+1にのみ映り込んでいる信号機３００にはＩＤ情報が付与されない。

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

次に、特徴点位置推定部１１は、既知のマップ中のマップポイントの位置を基準にし、フレーム画像ｆ_N-1を撮像したカメラ_N-1、カメラ_Nの姿勢及び自己位置を推定する。なお、本明細書でいう「マップポイント」は、マップに配置された特徴点を指す。自己位置の推定により、カメラ_N-1、カメラ_Nの自己位置は、既知の位置となる。特徴点位置推定部１１は、カメラ_N-1、カメラ_Nの自己位置に基づいて、位置が既知でない（未知である）特徴点の位置を推定する。さらに、特徴点位置推定部１１は、推定によって位置が得られた特徴点を既知のマップポイントとし、マップポイントの位置に基づいてカメラ_N+1の姿勢及び位置、すなわち、自己位置を推定する。本実施形態は、特徴点位置推定部１１によるこのような処理をフレーム画像ｆが入力される度に実行する。そして、フレーム画像のうち、キーフレームと判定されるフレーム画像ｆが入力されたタイミングでＳＬＡＭ空間内に特徴点が配置されたマップを作成する。

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

（マッチング部）
マッチング部１２は、複数の特徴点の相対的な位置関係と、複数のランドマークの実空間における位置の相対的な関係との類似性に基づいて、検出物体とランドマークとを対応付ける、対応付け工程を実行する。また、この際、マッチング部１２において実行される対応付け工程においては、ＳＬＡＭ空間と実空間とをそれぞれ座標変換し、その類似性が判断される。

すなわち、マッチング部１２は、図１に示すランドマークＤＢ５５に保存されている測位情報を読み出し、測位情報に基づいて検出された特徴点とランドマークとなる地物とを対応付けるマッチングの処理を実行する。図５から図７は、マッチングの処理を説明するための概念図である。図５等により説明するマッチングは、Ｇｅｏｍｅｔｒｉｃ Ｈａｓｈｉｎｇを用いる例を説明している。ただし、本実施形態は、このような構成に限定されるものでなく、任意の手法によって検出物体とランドマークとをマッチングするようにしてもよい。

図５は、ランドマークＤＢ５５に保存されている測位情報のベイシス対（Basis pair）の作成について説明するための図である。図６は、ベイシス対を共通のハッシュテーブルにマッピングした例を示している。図７は、特徴点の群を図５、図６で説明した処理と同様に処理し、共通のハッシュテーブルにマッピングすることを説明するための図である。本実施形態は、特徴点の群を予め設定された条件により地物と判断する。この条件は、例えば、図３に示すように、特徴点が複数存在する検出ボックスの範囲を地物と判断し、これを検出物体としてもよい。また、本実施形態では、以降、ランドマークと検出物体とを「オブジェクト」と総称する場合もある。

図５から図７は、処理を説明するために、複数のマップ６１から６６及びハッシュテーブル７１を示している。マップ６１から６６において、ドットの黒、白、網掛け（以下、「濃度」とも記す）の別はオブジェクトの種別に対応し、本実施形態において濃度が異なるドットはそれぞれ異なる種別のオブジェクトに対応する。本実施形態では、黒のドットで表される種別を第１種、白のドットで表される種別を第２種、網掛けのドットで表される種別を第３種とする。また、ハッシュテーブル７１においては、オブジェクトの別が括弧の種別によって表される。本実施形態の説明において、丸括弧（）は第１種、山括弧＜＞は第２種、角括弧［］は第３種に対応する。ここで、オブジェクトの種別とは、例えば、信号機や標識を指し、図４の例のように、信号機及び標識にＩＤ情報を付与した例ではオブジェクトの種別は「２」である。

マッチング部１２は、図５のマップ６１に示すように、先ず、ランドマークＤＢ５５から複数のランドマークを選択する。図５の例では、マップ６１において、ランドマークＬ１、Ｌ２、Ｌ３、Ｌ４、Ｌ５が選択されている。ランドマークＬ１からランドマークＬ５のうち、ランドマークＬ１、Ｌ４、Ｌ５は種別が同一であり、ランドマークＬ２、Ｌ３は、いずれも種別が他と相違している。

次に、マッチング部１２は、選択されたランドマークのうち、２つのランドマークをベイシス対として選択する。マップ６２は、ランドマークＬ４とランドマークＬ１とをベイシス対に選択した例を示し、マップ６３は、ランドマークＬ３とランドマークＬ１とをベイシス対に選択した例を示している。マップ６２において、ランドマークＬ１からＬ５に対応する各ドット間の距離は、ベイシス対となるランドマークＬ４、Ｌ１の距離に応じて拡大、または縮小される。また、マップ６３において、各ドット間の距離は、ベイシス対となるランドマークＬ３、Ｌ１の距離に応じて拡大、または縮小される。

次に、マッチング部１２は、拡大または縮小されたマップ６２、６３を共通のｘ－ｙ座標に変換する。ｘ－ｙ座標変換は、ベイシス対の一方を原点（０，０）に、他方を座標（１，０）にマッピングすることによって行われる。マップ６４においては、ランドマークＬ４に対応するドットが原点（０，０）にマッピングされ、ランドマークＬ１に対応するドットが座標（１，０）にマッピングされている。また、マップ６５においては、ランドマークＬ３に対応するドットが原点（０，０）にマッピングされ、ランドマークＬ１に対応するドットが座標（１，０）にマッピングされている。このようなマッピングは、マップ６２、６３を回転させることによって行われる。

次に、マッチング部１２は、図６に示すように、マップ６４、６５で得られた各ドットの位置関係をハッシュテーブル７１にマッピングする。マッピングは、ベイシス対と、オブジェクトの種別との関係が分かるように行われる。マップ６４では、ランドマークＬ４、Ｌ１をベイシス対とする場合、ランドマークＬ２が座標（３／２，０）に、ランドマークＬ３が座標（１，１／２）に、ランドマークＬ５が座標（３／２，－１）にマッピングされている。本実施形態では、座標（３／２，０）に該当するハッシュテーブル７１の欄にランドマークＬ２の種別を示す山括弧を使ってベイシス対＜４，１＞を示す。また、本実施形態は、座標（１，１／２）に該当するハッシュテーブル７１の欄に、ランドマークＬ３の種別を示す丸括弧を使ってベイシス対（４，１）を示し、座標（３／２，－１）に該当するハッシュテーブル７１の欄に、ランドマークＬ５の種別を示す角括弧を使ってベイシス対[４，１]を示す。さらに、本実施形態は、ベイシス対を構成するランドマークＬ４を、座標（０，０）に該当するハッシュテーブル７１の欄に[４，１]と示し、ランドマークＬ１を座標（０，１）に該当するハッシュテーブル７１の欄に[４，１]と示す。

また、マッチング部１２は、マップ６５に示すベイシス対とランドマークの種別との関係をハッシュテーブル７１にマッピングする。すなわち、マップ６５に示すように、ランドマークＬ３、Ｌ１をベイシス対とする場合、ランドマークＬ２が座標（１／２，１／２）に、ランドマークＬ４が座標（１，－３／２）に、ランドマークＬ５が座標（３／２，１／２）にマッピングされている。本実施形態は、座標（１／２，１／２）に該当するハッシュテーブル７１の欄にランドマークＬ２の種別を示す山括弧を使ってベイシス対＜３，１＞を示し、座標（１，－３／２）に該当するハッシュテーブル７１の欄にランドマークＬ４の種別を示す角括弧を使ってベイシス対[３，１]を示す。また、本実施形態は、ハッシュテーブル７１の座標（３／２，１／２）に該当する欄に、ランドマークＬ５の種別を示す角括弧を使ってベイシス対[３，１]を示し、ベイシス対を構成するランドマークＬ３を、座標（０，０）に該当するハッシュテーブル７１の欄に（３，１）と示し、ランドマークＬ１を座標（０，１）に該当するハッシュテーブル７１の欄に[３，１]と示す。

次に、マッチング部１２は、検出物体ＤＢ５７から所定の範囲にある複数の検出物体の座標を読み出す。所定の範囲は、先にランドマークＤＢから読みだされたランドマークが存在する範囲を含む。図７に示すように、本実施形態は、５つの検出物体Ｄａ、Ｄｂ、Ｄｃ、Ｄｄ、Ｄｅを検出物体ＤＢ５７から読み出す。そして、ランドマークについて処理したのと同様に、２つの検出物体によりベイシス対を複数生成する。図７のマップ６６においては、検出物体Ｄａ、Ｄｂがベイシス対を生成し、検出物体Ｄａに対応するドットが原点（０，０）に、検出物体に対応するドットが座標（１，０）にマッピングされている。本実施形態は、他の検出物体についてもベイシス対を作成し、適宜拡大、縮小、あるいは回転させて座標にマッピングする。そして、マッチング部１２は、ランドマークと同様に、マッピングされた検出物体に対応するドットを、座標に該当するハッシュテーブル７１の欄にマッピングする。

以上の処理によれば、図７のハッシュテーブル７１に示すように、同一の欄にマッピングされるランドマークと検出物体とが存在するようになる。マッチング部１２は、同一の欄にマッピングされる回数が最も多く、かつオブジェクトの種別が同一のランドマークと検出物体とが一致すると推測する。本実施形態においては、ランドマークＬ４、Ｌ１のベイシス対と、検出物体Ｄａ、Ｄｂのベイシス対とがマッピングされている欄が５か所あり、この５か所のいずれにおいてもランドマークＬ４、Ｌ１のベイシス対と、検出物体Ｄａ、Ｄｂのベイシス対の種別は一致する。これに対し、ランドマークＬ３、Ｌ１のベイシス対と、検出物体Ｄａ、Ｄｂのベイシス対とがマッピングされている欄は２か所である。この２か所のいずれにおいても、ランドマークＬ３、Ｌ１のベイシス対と検出物体Ｄａ、Ｄｂのベイシス対との種別は一致するものの、共にマッピングされる欄の数がランドマークＬ４、Ｌ１のベイシス対及び検出物体Ｄａ、Ｄｂのベイシス対よりも少なくなっている。このような場合、マッチング部１２は、ランドマークＬ４が検出物体Ｄａに一致し、ランドマークＬ１が検出物体Ｄｂに一致すると推測する。

さらに、マッチング部１２は、検出物体ＤｃがランドマークＬ５に一致し、検出物体ＤｄがランドマークＬ２に一致し、検出物体ＤｅがランドマークＬ３に一致すると推定する。このような推定は、例えば、先に推定された検出物体ＤａとランドマークＬ４との一致、及び検出物体ＤｂとランドマークＬ１との一致を基準にして行ってもよい。また、推定は、他のオブジェクトを使ってさらにベイシス対を作成し、作成されたベイシス対をハッシュテーブルにマッピングして行ってもよい。

（自己位置推定部）
検出物体とランドマークとの対応付けの終了後、自己位置推定部１３は、撮像されたフレーム画像のうちのキーフレーム画像から推定される自己の位置を補正する。補正は、キーフレーム画像内にランドマークが実際に撮像された位置と、本来撮像されるであろう再投影位置との差分を算出し、算出された差分である再投影誤差が減少するように、カメラの位置及び角度に係る撮像情報を補正（最適化）することによって行われる。本実施形態の撮像情報は、少なくともカメラの角度及び位置に関する情報を含んでいればよく、適宜他の情報を含んでいてもよい。なお、再投影位置は、キーフレーム画像にランドマークの実位置を投影することによって決定される。再投影位置の補正は、ＳＬＡＭ空間において行われる。

式（１）は、本実施形態の自己位置の補正に用いられる演算式であって、バンドル調整を採用している。式（１）は、フレーム位置ｔ、カメラの角度ｒ、ランドマーク（点群）の位置ｐのコスト関数である。フレーム位置ｔは並進座標系で表され、カメラの角度ｒは回転座標系で表される。式（１）の右辺は第１項及び第２項を含み、第１項は公知のバンドル調整にも含まれるOＲＢ－ＳＬＡＭの基本項であり、マップポイントの再投影誤差を示している。第２項は、本実施形態のランドマーク補正項であり、ランドマークの再投影誤差を示している。式（１）中のｉはキーフレーム画像の番号を示し（何番目のキーフレーム画像か）、ｊはキーフレーム画像内の特徴点の番号（何番目の点か）を示す。ｉｊの添え字は、ｉ番目のキーフレーム画像に撮像されたｊ番目の特徴点を意味している。

（出力部）
出力部３は、上記の処理によって推定された自己位置、すなわちカメラの位置を出力する。自己位置の出力にあたり、本実施形態は、ＳＬＡＭ空間で表される補正後の自己位置を相似変換等によってＧＰＳ空間に置き換えて、実空間のマップに重ねるデータを生成するようにしてもよい。そして、生成されたデータをコンピュータ等のディスプレイ画面に表示するようにしてもよい。このような処理によれば、実空間を走行する車両の正確な走行ルートを視認することが可能になる。また、以上の処理を車両を走行させながらリアルタイムで行う場合には、走行中の車両のルートを正確に推定し、例えば、カーナビゲーションや車両の自動運転、あるいは遠隔操作等に利用することができる。また、出力部３は、カメラを搭載した車両の位置を音声で出力してもよい。さらに、出力部３は、推定された自己位置をテキスト情報や座標により記録し、所望のタイミングで出力可能な構成であってもよい。このようにすれば、本実施形態の自己位置推定装置を車両のレコーダとして利用することが可能になる。

次に、以上説明した処理を実行する機能をスレッドの観点から説明する。図８は、図１の機能ブロック図の演算処理部１０を処理のスレッド毎に表現した図である。図８に示すように、図１の演算処理部１０の処理は、トラッキング部４１、ローカルマッピング部４２及びループクロージャ―部４３、ランドマーク補正部４４の４つのスレッドに分けられる。トラッキング部４１は、複数のフレーム画像ｆを順次入力する。そして、例えば図３で説明したように、入力されたフレーム画像ｆから特徴点を抽出する。また、図４（ａ）、図４（ｂ）、図４（ｃ）に示したように、移動中のカメラから送られてくるフレーム画像を、直前に入力された特徴点とマッチングし、特徴点の検出とマッチングを繰り返しながらマップポイントの位置とカメラの自己位置及び姿勢を推定する。

このような処理の過程において、トラッキング部４１は、複数のフレーム画像ｆからマップの修正に用いられるキーフレーム画像ｆｋを抽出する。抽出されたキーフレーム画像ｆｋは、ローカルマッピング部４２に入力される。ローカルマッピング部４２は、作成されるマップに記録されるマップポイントを作成する。次に、マップポイントをキーフレーム画像ｆｋに投影して再投影位置を求める。さらに、算出された再投影誤差を式（１）を使って最小値化し、カメラの姿勢及び位置を補正する。ローカルマッピング部４２によって算出される再投影誤差は、式（１）における右辺の第１項によって表される。

上記処理を実行することにより、本実施形態は、カメラの自己位置と、この自己位置において撮像されるマップポイントとの位置とを対応付け、処理を繰り返すことによって対応付けの精度を高めることができるようになる。つまり、本実施形態は、より多くのキーフレーム画像ｆｋの間で自己位置を再計算し、再投影誤差を次第に小さくして自己位置の精度を高めることができる。ループクロージャ―部４３は、キーフレームから画像ｆｋからループ閉じ込めを検出し、ループ閉じ込めがあった場合、フレーム画像ＤＢに保存されている全てのフレーム画像ｆｋから自己位置を推定し、カメラ姿勢及び作成中のマップを最適化する。

トラッキング部４１、ローカルマッピング部４２及びループクロージャ―部４３は、以上の処理を別スレッドで並列に実行する。トラッキング部４１で行われる処理は、３つのスレッドの中で計算量が相対的に少なく、処理頻度が高い。一方、ローカルマッピング部４２によって行われる処理は計算量がトラッキング部４１よりも大きく、処理の頻度が低くなる。また、ループクロージャ―部４３において行われる処理は、３つのスレッドのうち計算量が最も大きく、処理の頻度が低くなる。このようにスレッドを分けて並列に処理を行うことにより、本実施形態は、いずれかの処理に係る負荷が大きくなって他の処理に支障がでることを抑え、自己位置推定装置１におけるメモリの利用効率を高めることができる。

以上の処理によって生成されたマップは、ランドマーク補正部４４によってさらに補正され、その精度が高められる。ランドマーク補正部４４は、図５から図７で説明したように、検出物体とランドマークとをマッチングする。そして、ランドマーク補正部４４は、検出物体とランドマークとの対応付けの終了後、撮像されたフレーム画像のうちのキーフレーム画像から推定されるカメラの位置及び姿勢を補正する。補正は、式（１）の右辺の第２項の値が最小値となるようにカメラの並進座標系で表される位置及び回転座標系で表される角度を微調整することによって行われる。

以上説明した処理は、公知のバンドル調整を使ってマップの作成及び最適化を実行し、この後、さらにランドマークの補正を行う例である。ランドマークによる補正をマップの作成と並行してリアルタイムで行う場合、例えば、ローカルマッピング部４２の最後端において行ってもよい。このような場合、ローカルマッピング部４２は、再投影誤差を最小値化してカメラの姿勢及び位置を補正した後、特徴点によって決定される検出物体をランドマークに対応付けて補正する。このような場合、ループクロージャ―部４３は、バンドル調整とランドマーク補正の両方が行われたマップからループを検出し、カメラの姿勢を最適化し、マップをさらに最適化する。

［自己位置推定方法及びプログラム］
次に、以上説明した自己位置推定装置において動作するプログラムにより実行される自己位置推定方法について説明する。図９から図１１は、自己位置推定装置１において実行される処理を説明するためのフローチャートである。図９に示すように、自己位置推定装置１は、車両に搭載された図示しないカメラによって撮影された動画から、一定の間隔で静止画像を抽出することによって順次フレーム画像を取得する（ステップＳ２０１）。取得されたフレーム画像は、フレーム画像ＤＢ５６に保存される。

次に、特徴点抽出部１４は、例えば、図３により説明したように、フレーム画像に注目点を設定し、この注目点の周囲に閾値以上の明るい点が所定の数以上あるか否か判断する（ステップＳ２０２）。そして、明るい点が所定の数以上ある場合、この注目点が特徴点として認識される（ステップＳ２０２：ＹＥＳ）。特徴点がフレーム画像中にない場合（ステップＳ２０２：ＮＯ）、特徴点抽出部１４は、このフレーム画像の以降の処理を行わず、次のフレーム画像を取得する。そして、特徴点抽出部１４は、特徴点があるフレーム画像の中からキーフレームと判定されるフレーム画像を決定する（ステップＳ２０３）。キーフレーム画像は、推定によって得られた位置に特徴点が配置されるＳＬＡＭ空間のマップを作成、あるいは補正することに使用される。

次に、特徴点抽出部１４は、特徴点のうちの図３に示す検出ボックスに内包されると認識されるものを抽出する（ステップＳ２０４）。抽出された特徴点は、検出物体ＤＢ５７に検出物体の座標の一部分として保存される（ステップＳ２０５）。

特徴点位置推定部１１は、図４（ａ）から図４（ｃ）により説明したように、ＳＬＡＭ座標等と共に既知のマップ中の特徴点を基準にし、各フレーム画像を撮影した際の自己位置を推定する。さらに、推定された自己位置を既知として、既知でない特徴点の３次元の位置を推定する（ステップＳ２０６）。特徴点位置推定部１１は、特徴点及び自己位置の一方を既知として他方を推定する処理を交互に繰り返し、特徴点の位置をマップＤＢ５９に保存する（ステップＳ２０７）。

次に、自己位置推定部１３は、キーフレーム画像内にマップポイントが本来撮像されるであろう再投影位置を算出する（ステップＳ２０８）。そして、このマップポイントが撮像された位置と再投影位置との差分をとり、再投影誤差を算出する（ステップＳ２０９）。さらに、自己位置推定部１３は、例えば式（１）を使い、算出された再投影誤差が小さくなるように、ステップＳ２０６において推定された自己位置を繰り返し補正する。補正の結果、再投影誤差が最小値をとると、カメラ姿勢を含む自己位置の最適化が終了する（ステップＳ２１０）。さらに、演算処理部１０は、例えばフレーム画像の入力の有無、あるいは処理のスイッチオフ等によって処理の終了を判定する（ステップＳ２１１）。ステップＳ２１１において、処理が終了しないと判断されなかった場合（ステップＳ２１１：ＮＯ）、特徴点抽出部１４が次のフレーム画像を取得して処理が続けられる。一方、処理が終了したと判断された場合には処理は終了される（ステップＳ２１１：ＹＥＳ）。

図１０は、図９により説明した処理の後に行われる、ランドマークとのマッチングを含む処理を説明するためのフローチャートである。図１１は、図１０中にサブルーチンとして示す検出物体とランドマークとのマッチングを説明するためのフローチャートである。

先ず、図１０のステップＳ３０１のマッチング処理を説明する。図１１に示すように、マッチング部１２は、ランドマークＤＢ５５から複数のデータを読み出し、このうちベイシス対となる２つのランドマークを選択する（ステップＳ４０１）。そして、ベイシス対を構成する２つのランドマークの一方の測位情報が原点（０，０）に、他方の測位情報が（１，０）にマッピングされるように複数のランドマークの座標を変換する（ステップＳ４０２）。座標変換後の各ランドマークは、ハッシュテーブルにマッピングされる（ステップＳ４０３）。

次に、マッチング部１２は、ランドマークＤＢ５５からデータを読み出した複数のランドマークの全ての組み合わせでベイシス対を作成し、ハッシュテーブルへのマッピングが完了したか否かを判断する（ステップＳ４０４）。この判断の結果、全てのベイシス対の組み合わせでマッピングが完了している場合（ステップＳ４０４：ＹＥＳ）、マッチング部１２は、検出物体のマッピングの処理に移行する。また、マッピングが完了していない場合（ステップＳ４０４：ＮＯ）、マッチング部１２は、ベイシス対を構成する２つのランドマークを変更し（ステップＳ４０１）、他のランドマークのベイシス対を使ってステップＳ４０２からＳ４０３の処理を繰り返す。

ステップＳ４０４においてＹＥＳと判断された場合、マッチング部１２は、検出物体ＤＢから複数の検出物体のデータを読み出し、ランドマークと同様に、ベイシス対となる２つの検出物体を選択する（ステップＳ４０５）。そして、複数のランドマークの座標を変換し（ステップＳ４０６）、座標変換後の各検出物体を、ランドマークがマッピングされているハッシュテーブルにマッピングする（ステップＳ４０７）。このような処理は、読みだされた検出物体の全ての組み合わせのベイシス対に基づくマッピングが終了するまで繰り返される（ステップＳ４０８：ＮＯ）。

ステップＳ４０８においてＹＥＳと判断された場合、マッチング部１２は、図７に示すように、ハッシュテーブルの同一の欄にマッピングされる回数が最も多く、かつ種別が同一のランドマークと検出物体の組み合わせを検出する。そして、検出された組み合わせの検出物体とランドマークとを対応付ける（ステップＳ４０９）。マッチングの終了後、ランドマークのＧＰＳ座標は、ＳＬＡＭ座標に変換され、マッチングの処理が終了する（ステップＳ４１０）。

ランドマークとのマッチング終了後、図１０に示すように、自己位置推定部１３は、キーフレーム画像内にランドマークが本来撮像されるであろう再投影位置を算出する（ステップＳ３０２）。そして、ランドマークが実際に撮像された位置と再投影位置との差分をとり、再投影誤差を算出する（ステップＳ３０３）。さらに、自己位置推定部１３は、例えば式（１）を使い、算出された再投影誤差が小さくなるように、図９のステップＳ２１０において最適化された自己位置を繰り返し補正する。補正の結果、再投影誤差が最小値をとると、カメラ姿勢及び自己位置の最適化が終了する（ステップＳ３０４）。

以上の処理の後、自己位置推定部１３は、最適化された自己位置を基準にして検出物体のＳＬＡＭ座標を算出し（ステップＳ３０５）、ＳＬＡＭ座標をさらにＧＰＳ座標に変換する（ステップＳ３０６）。さらに、演算処理部１０は、処理の終了を判定し（ステップＳ３０７）、ステップＳ３０７において、処理が終了しないと判断されなかった場合（ステップＳ３０７：ＮＯ）、マッチング部１３による検出物体とランドマークとの対応が続けられる。一方、処理が終了したと判断された場合には、処理は終了される（ステップＳ３０７：ＹＥＳ）。以上の処理により、本実施形態は、カメラが移動しながら動画を撮影した環境の実空間のマップを高い精度で作成することができる。

以上説明した本実施形態は、ＳＬＡＭ空間において推定された自己位置を再投影誤差に基づいて補正することに加え、ランドマークとなる事物の実空間の座標により自己位置を補正している。このような構成により、本実施形の自己位置推定方法等は、実空間に即した自己位置を高い精度で取得し、この自己位置に基づいて地物の位置を正確に判定することができる。このような本実施形態は、特に屋外における実空間のマップを作成することに有効である。

さらに、本発明の実施形態は、作成されたマップの精度が十分に高まった後、自己位置とフレーム画像とを対応付けてデータベース化しておいてもよい。このようにすれば、撮影されたフレーム画像を順次このデータベースのフレーム画像と対照し、撮影されたフレーム画像と一致するデータベースのフレーム画像に対応付けられた自己位置を、フレーム画像を撮像した際のカメラの自己位置と判定するように運用することが可能となる。

１ 自己位置推定装置
２ 受信部
３ 出力部
１０ 演算処理部
１２ マッチング部
１３ 自己位置推定部
１４ 特徴点抽出部
４１ トラッキング部
４２ ローカルマッピング部
４３ ループクロージャ―部
４４ ランドマーク補正部
５５ ランドマークデータＤＢ
５６ フレーム画像ＤＢ
５７ 検出物体ＤＢ
５９ マップＤＢ
６１，６２，６３，６４，６５，６６ マップ
７１ ハッシュテーブル
１００，３００ 信号機
２００ 標識

Claims (8)

1. 撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け工程と、
   前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定工程と、
   前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定工程と、を含む、自己位置推定方法。
2. 前記第１の空間における前記検出物体の検出は、撮像によって生成された複数の前記撮像画像から、前記撮像画像の少なくとも一部に表示されている共通の複数の前記検出物体を順次検出することによって行われる、請求項１に記載の自己位置推定方法。
3. 前記撮像推定工程においては、算出された前記差分の値が減少するように、前記撮像情報を補正する、請求項１または２に記載の自己位置推定方法。
4. 前記対応付け工程においては、複数の前記検出物体の相対的な位置関係と、複数の前記ランドマークの前記第２の空間における位置の相対的な関係との類似性に基づいて、前記検出物体と前記ランドマークとを対応付ける、請求項１から３のいずれか一項に記載の自己位置推定方法。
5. 前記対応付け工程においては、複数の前記検出物体の相対的な位置関係と、複数の前記ランドマークの前記第２の空間における位置の相対的な関係との類似性に基づいて、前記検出物体と前記ランドマークとを対応付けて、
   前記対応付けは、前記第１の空間と前記第２の空間とを座標変換して前記類似性を判断する、請求項１から３のいずれか一項に記載の自己位置推定方法。
6. 前記第１の空間は、相対的な座標により表される空間であり、前記第２の空間は測地系に基づく実空間である、請求項１から５のいずれか一項に記載の自己位置推定方法。
7. 撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け機能と、
   前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定機能と、
   前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定機能と、を、コンピュータに実行させる自己位置推定プログラム。
8. 撮像画像に基づいて設定された第１の空間において検出され、位置が推定された検出物体を、前記第１の空間の座標と異なる他の座標によって規定される第２の空間において位置が既知であるランドマークと対応付ける対応付け部と、
   前記ランドマークの前記既知の位置に基づいて、前記ランドマークに対応付けられた前記検出物体が前記撮像画像において撮像される予定の位置である再投影位置を決定する決定部と、
   前記再投影位置と、前記撮像画像に撮像された前記検出物体の実際の位置との差分を最小化することにより、少なくとも前記撮像画像の撮像位置及び撮像角度を含む撮像情報を推定する撮像推定部と、を含む、自己位置推定装置。

Images