JP6821154B2 - 基準映像地図を用いた自己位置姿勢標定装置 - Google Patents

Description

本発明は、例えば自動車などの各種車両や航空機，船舶等の移動体の自動運転、ロボット等の自動走行などにおいて、移動する車両等の移動体が自らの位置と姿勢をリアルタイムに標定するための自己位置姿勢標定装置に関する。

一般に、自動車や航空機，船舶等の移動体の自動運転においては、移動する車両等が、自らの位置と姿勢を把握・標定するための自己位置姿勢標定技術が重要となる。
ここで、このような移動体の自己位置姿勢標定にはいくつかの方法がある。
具体的には、移動車両等の自己位置姿勢標定装置としては、原理的には航空機などに利用される高価で高精度なＩＭＵ／ＧＹＲＯで取得した６変数（位置座標（Ｘ，Ｙ，Ｚ）とそれぞれの座標軸の回転角（Φｘ，Φｙ，Φｚ）の六個の自由度のベクトル（変数））データを用いることで可能である。しかしながら、実際問題として、自動運転に耐えうる精度を出すには、装置・設備等が非常に高額となり、実用的ではない。
また、ＧＮＳＳ（ＧＰＳ）は、一般に自己位置標定として普及した装置であるが、座標のみの３変数であるから、姿勢を含む６変数を取得することはできない。

近年、ＬＩＤＡＲ方式（Light Detection and Ranging，Laser Imaging Detection and Ranging）と呼ばれる自己位置標定方法が主流となっている。これはレーザーパルスをスキャンして三次元空間からの散乱光を発射位置で受光し、時間差から距離を計測することで、点群を作成して、三次元空間を三次元座標を持った点群で密に作り上げる技術である。
このような点群を用いる自己位置姿勢標定技術としては、例えば、車両等の自動運転においてレーザー点群を三次元地図として、車載のレーザー装置からのレーザースキャンデータと比較して、自己位置姿勢標定を実現することが開示されている（特許文献１）。

また、最近では、このような点群を処理して、自己位置姿勢標定と環境地図作成を同時に行う技術が提案されている。これはＳＬＡＭ（Simultaneous Localization And Mapping）と呼ばれる技術で、近年普及している。
このＳＬＡＭから発展した技術として、車載カメラの映像からレーザー点群と同じように点群を作り出し、画像の全体近くを三次元点として表示するＶ−ＳＬＡＭ（VisualＳＬＡＭ）がある。Ｖ−ＳＬＡＭでは、車載カメラからの映像を直接加工して生成した点群から、自己位置姿勢を標定することが試行されている。

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

しかしながら、上記のような従来の自己位置姿勢標定の方法では、膨大な費用と莫大なデータ量が発生するという問題があった。
すなわち、特許文献１で提案されているようなレーザー方式、すなわちＬＩＤＡＲ方式によって三次元地図を生成すると、莫大な費用がかかる上に、さらに三次元地図は環境の変化等に応じて更新しなければならないため、その度に莫大な費用がかかることになる。
また、ＬＩＤＡＲ方式の最大の欠点は、三次元点群を作成し、データ管理するのに莫大な手間と費用がかかることである。さらに更新にも同様な手間と費用がかかり、実用的ではなかった。
このように、従来の自己位置姿勢標定の方法では、膨大な量の三次元点のデータを管理することになり、扱うデータ量が莫大なものとなり、実用的ではなかった。

さらに、従来の自己位置姿勢標定技術には、精度の点でも問題があった。
すなわち、特許文献１に提案されているような方法により三次元地図が生成できたとしても、それを参照して、自動運転車両に取り付けた高額の装置等によって、自己位置を自動演算で求めなければならない。このような自己位置の演算技術は、未だその方式は模索の段階であり、演算のための装置が高額である割には、精度と安定度に欠けるという状況であった。

このように、従来提案されている自己位置姿勢標定の技術では、三次元地図の作成に膨大な費用がかかり、また、自己位置を求めるための装置等にも費用がかかり、さらに、そのためのデータも莫大な量となってしまうという問題があった。
このため、自動運転を普及させるには、より簡便に、安価に、正確な自己位置姿勢標定を実現する必要があった。
しかしながら、現在まで、このような従来の自己位置姿勢標定が有する課題を有効に解決し得る技術や提案はなされていなかった。

本願発明者は、このような自己位置姿勢標定技術が有する課題を解決し得る発明として、鋭意研究の結果、予め撮影した基準映像に基づいて、基準映像のカメラ位置と姿勢角を示すＣＶ値（位置と姿勢の６変数）を高精度に求めたＣＶ映像地図を生成し、その基準となるＣＶ映像地図の三次元座標を、自己位置姿勢標定の対象となる車両等から撮影した目的画像中に移植・移転させることで、車両等の三次元位置座標を簡易かつ低コストで、高速かつ高精度に求め得ることに想到した。

また、基準映像地図であるＣＶ映像地図と、目的画像との比較で、自己位置姿勢標定に必要な高精度のＣＶ値（６変数）を目的画像のフレーム単位で取得できるだけでは、画像を利用する場合、ＣＶ値は画像のフレーム単位以上のサンプル密度では取得できない。これは画像を使う場合の避けられない原理的な課題である。
そこで本願発明者は、この課題をも解決するために、機械センサーで取得可能な６変数を用いて、画像のフレーム間を内挿して、時間的に連続するＣＶ値を取得し得ることに想到した。しかも、機械センサー自体は、安価な低精度の機械センサーであってもそれが可能であるという優れた特徴点を見出した。

すなわち、本発明は、以上のような従来の技術が有する問題を解決するために提案されたものであり、各種車両や航空機，船舶等の自動運転、ロボット等の自動走行などにおいて、移動する車両等が、自らの位置と姿勢をリアルタイムに標定するための自己位置姿勢標定を、簡易かつ低コストで、高速かつ高精度に求めることができるように、ＣＶ映像地図を基準映像地図として用いた自己位置姿勢標定装置の提供を目的とする。

上記目的を達成するため、本発明のＣＶ映像地図を基準映像地図として用いた自己位置姿勢標定装置は、所定の映像取得手段で撮影された基準映像に基づいて、当該基準映像のカメラ位置と姿勢の三次元座標値を示すＣＶ（カメラベクトル）値を求めるＣＶ演算を行い、前記基準映像に前記ＣＶ値を付加したＣＶ映像地図を生成するＣＶ映像地図作成手段と、前記ＣＶ映像地図を記憶するＣＶ映像地図データベースと、前記ＣＶ映像地図データベースに記憶されたＣＶ映像地図を基準画像とし、目的移動体に備えられた所定の画像取得手段で撮影された目的画像を前記ＣＶ映像地図と比較して、当該目的画像とＣＶ映像地図の同一箇所を示す複数の特徴点を自動的に対応させることにより、当該目的画像のＣＶ値を取得する自己位置姿勢標定手段と、を備え、前記自己位置姿勢標定手段が、前記ＣＶ映像地図と前記目的画像を組み合わせて、前記ＣＶ映像地図及び前記目的画像に含まれる二次元特徴点及び三次元特徴点を一体として前記ＣＶ演算を行い、三次元座標が既知の前記ＣＶ映像地図の特徴点の三次元座標は変数としてのＣＶ演算を行うことなく固定して、前記ＣＶ映像地図及び前記目的画像に含まれる全ての特徴点についてのＣＶ演算を行うことで、前記目的画像のＣＶ値を取得するとともに、前記ＣＶ映像地図及び前記目的画像に含まれる全てのＣＶ値から、前記目的画像のＣＶ値を分離して取り出し、自己位置標定のＣＶ値とする構成としてある。

本発明の基準映像地図を用いた自己位置姿勢標定装置によれば、予め用意した基準映像について、カメラ位置と姿勢の三次元座標値を示すＣＶ（カメラベクトル）値を求めるＣＶ演算を行い、前記基準映像に前記ＣＶ値を付加したＣＶ映像地図を生成し、このＣＶ映像地図に基づいて、対象となる目的画像に対して三次元座標を付加・移植することにより、目的画像の自己位置姿勢標定を、高速かつ高精度に行うことができる。
これにより、車両等の自動運転，自動走行等に必要となる、移動体の位置と姿勢をリアルタイムに標定するための自己位置姿勢標定を、簡易かつ低コストで、高速かつ高精度に求めることが可能となる。

本発明の基準映像地図を用いた自己位置姿勢標定装置において、基準映像のＣＶ演算を行うＣＶ演算手段（ＣＶ映像地図作成装置）の一実施形態の基本構成を示すブロック図である。 図１に示すＣＶ演算手段で使用する全周ビデオ映像を撮影する手段を示す概略図であり、屋根部に全周カメラを搭載した車輌の斜視図である。 図１に示すＣＶ演算手段で使用する全周ビデオ映像を撮影する手段を示す概略図であり、（ａ）は屋根部に全周カメラを搭載した車輌の正面図、（ｂ）は同じく平面図である。 全周カメラで撮影される映像から得られる変換画像を示す説明図であり、（ａ）は球面画像が貼り付けられる仮想球面を、（ｂ）は仮想球面に貼り付けられた球面画像の一例を、（ｃ）は（ｂ）に示した球面画像をメルカトール図法に従って平面展開した画像を示している。 本発明の一実施形態に係るＣＶ演算手段おける具体的なカメラベクトルの検出方法を示す説明図である。 本発明の一実施形態に係るＣＶ演算手段における具体的なカメラベクトルの検出方法を示す説明図である。 本発明の一実施形態に係るＣＶ演算手段における具体的なカメラベクトルの検出方法を示す説明図である。 本発明の一実施形態に係るＣＶ演算手段によるカメラベクトルの検出方法における望ましい特徴点の指定態様を示す説明図である。 本発明の一実施形態に係るＣＶ演算手段により得られる特徴点の三次元座標とカメラベクトルの例を示すグラフである。 本発明の一実施形態に係るＣＶ演算手段により得られる特徴点の三次元座標とカメラベクトルの例を示すグラフである。 本発明の一実施形態に係るＣＶデータ演算手段により得られる特徴点の三次元座標とカメラベクトルの例を示すグラフである。 本発明の一実施形態に係るＣＶ演算手段において、カメラから特徴点の距離に応じて複数の特徴点を設定し、それを隣接するフレームに亘って追跡し、複数の演算を繰り返し行う場合を示す説明図である。 本発明の一実施形態に係るＣＶデータ演算手段で求められたカメラベクトルの軌跡をビデオ映像中に表示した場合の図である。 本発明の一実施形態に係る基準映像地図を用いた自己位置姿勢標定装置の基本構成を示すブロック図である。 図１４に示す基準映像地図を用いた自己位置姿勢標定装置におけるＣＶ値の移転処理動作の詳細を示すブロック図である。 図１５に示す基準映像地図を用いた自己位置姿勢標定装置におけるＣＶ値の移転処理動作の具体例を模式的に示す説明図である。 本発明の一実施形態に係る基準映像地図を用いた自己位置姿勢標定装置により得られる目的画像のＣＶ値を機械センサーで得られる６変数により高精度化する場合の処理を模式的に示した説明図であり、（ａ）は目的画像を構成する複数フレームの全体を、（ｂ）は（ａ）に示す複数フレームの一部を拡大して示したものである。 本発明の一実施形態に係る基準映像地図を用いた自己位置姿勢標定装置による移動体の自動運転システムのシステム構成を示す機能ブロック図である。

以下、本発明に係る基準映像地図を用いた自己位置姿勢標定装置の好ましい実施形態について、図面を参照しつつ説明する。
ここで、以下に示す本発明の基準映像地図を用いた自己位置姿勢標定装置は、プログラム（ソフトウェア）の命令によりコンピュータで実行される処理，手段，機能によって実現される。プログラムは、コンピュータの各構成要素に指令を送り、以下に示すような所定の処理や機能、例えば、映像中の基準となる特徴点（基準点）やその他の特徴点の自動抽出，抽出した基準点の自動追跡，基準点の三次元座標の算出，ＣＶ（カメラベクトル）値の演算，基準映像と目的画像の対応基準点の検出，基準映像・目的画像間のＣＶ値の移植・統合，機械センサーで得られた６変数によるフレーム間の内挿入等を行わせる。このように、本発明における各処理や手段は、プログラムとコンピュータとが協働した具体的手段によって実現される。

なお、プログラムの全部又は一部は、例えば、磁気ディスク，光ディスク，半導体メモリ，その他任意のコンピュータで読取り可能な記録媒体により提供され、記録媒体から読み出されたプログラムがコンピュータにインストールされて実行される。
また、プログラムは、記録媒体を介さず、通信回線を通じて直接にコンピュータにロードし実行することもできる。

［ＣＶ映像地図］
以下に示す本発明の一実施形態に係る基準映像地図を用いた自己位置姿勢標定装置は、例えば自動車などの各種車両や航空機，船舶等の移動体の自動運転、ロボット等の自動走行などにおいて、移動する車両等の移動体が自らの位置と姿勢をリアルタイムに標定するための手段である。
具体的には、本実施形態に係る自己位置姿勢標定装置では、自己位置姿勢標定を実現するために、三次元地図となるＣＶ（カメラベクトル）映像地図を用いている。

一般に、移動体の自動走行、例えば車両の自動走行やロボットの自動移動走行の現状としては、大きく分類して、高精度な三次元（３Ｄ）地図を必要とせず、自ら周囲の環境を判断して走行する自律走行方式と、移動体の案内等のための高精度な三次元地図を必要とする三次元地図案内方式の二種類の方式が存在する。本発明は後者の三次元地図を案内として利用する方式を採用している。
そして、本発明では、各種車両や航空機等の自動運転、ロボット等の自動走行などにおいて、移動する車両等が、自ら取り込んだ画像や映像と、すでに用意してある三次元地図を参照して、その基準となる三次元地図と、移動対象（自動走行する車両等）に取り付けたカメラから取り込んだ画像（目的画像）と、さらに機械センサーで取得される６変数（三次元位置座標と回転座標の計６変数）を取得して、それらを自動的に比較し、補正して、自らの位置と姿勢をリアルタイムに標定する、自己位置姿勢標定を実現するものである。

まず、自己位置姿勢標定の基準となる三次元地図であるＣＶ映像について説明する。
移動体の目的の走行以前に、基準となるＣＶ映像地図の作製用の車両等に撮影カメラを設置し、動画映像、又は連続する静止画を取得し、その画像の中に特徴点を抽出するなどして、数学的演算により、全フレームのカメラ位置と姿勢を演算で求める。
具体的にはカメラ位置と姿勢を６変数、具体的には、カメラの位置座標（Ｘ，Ｙ，Ｚ）とそれぞれの座標軸の回転角（Φｘ，Φｙ，Φｚ）の六個の自由度のベクトル（カメラベクトル：ＣＶ）で表し、それを映像の各フレームに一対一に対応させることで、ＣＶ映像を生成することができる（後述する図１〜１３参照）。
このＣＶ映像を基準として用いるものが、自動走行案内用のＣＶ映像地図である。

ここで、目的となるカメラの位置と姿勢を示す６変数とは、座標［Ｘ，Ｙ，Ｚ］と姿勢［Φｘ，Φｙ，Φｚ］の計６種類の変数である。
上述したＶ−ＳＬＡＭも、レーザー点群から生まれた技術のため、三次元点群を作り、それをデータとして持つことになる。これは画像を利用する点で本発明と一見似ているように見えるが、画像内の全域に巨大な３次元点群を持つか、持たないかの重要な違いがあり、巨大な点群を持たない本発明とは大きく異なる。
本発明は、直接には三次元点群データを持たず、すべての三次元情報をカメラ位置と姿勢に集約することで、データを極端に軽くし、一手間かけることで、いつでも任意の点の三次元座標を取得できるようにしたものである。こうすることで、データ量を極端に軽くし、演算処理も効率化できるようになる。

すなわち、本実施形態に係る自己位置姿勢標定装置は、目的移動体の位置と姿勢を示す６変数を取得するものである。この６変数の取得とは、上述のとおり、三次元位置座標を示す［Ｘ，Ｙ，Ｚ］と姿勢を示す［Φｘ，Φｙ，Φｚ］の６個の変数を決定することである。
具体的には、まず、自動走行を目的とする移動する物体に取り付けた、安価な機械センサー、例えばＩＭＵ／ＧＹＲＯ／ＧＮＳＳ（ＧＰＳ）などから取得できる低精度なデータに基づいてＣＶ値を取得しておくことができる。あるいは、目的に応じて、ＣＶ値６変数のうちの目的の変数だけを取得することも可能である。
ここで、機械センサーを安価で低精度としたのは、高価で高精度では、上述した従来技術と同様に現実的ではないからである。
ＩＭＵ／ＧＹＲＯ／ＧＮＳＳ（ＧＰＳ）などから取得したＣＶ値は、精度は悪いが時間的に連続で出力できることが特徴であり、この点において、画像のフレーム単位で取得されるＣＶ値と比較して優れた長所である。

そして、本実施形態の自己位置姿勢標定装置は、自動走行を目的とする移動する物体に取り付けたカメラからの映像又は連続する画像（目的画像）と、既に用意されているＣＶ映像地図を三次元地図として、両者を比較することで、自己位置姿勢標定する装置であり、これをＣＶ映像参照型自己位置姿勢標定装置と呼ぶ。
ＣＶ映像地図は、通常は高精度のＧＮＳＳ（ＧＰＳ）により、絶対座標を付与することができる。ここでのＧＮＳＳは、上述した低精度のＧＮＳＳとは異なる別のＧＮＳＳであり、高価で高精度のＧＮＳＳを想定している。
さらに、ＩＭＵ／ＧＹＲＯ／ＧＰＳから取得されるＣＶ値は、フレーム単位（又はその整数倍）で取得されたＣＶ値（既に用意されているＣＶ映像地図を目的画像と比較演算することで得られるＣＶ値）は、時間的に不連続な部分を埋めるために、機械センサーからのＣＶ値を、不連続期間の両端のＣＶ値に合致させるように比例配分するなどして内挿することができる。機械センサーによるＣＶ値は、極短時間（数秒程度）では高精度であるが、長時間では誤差が累積されるため、実用的ではないという性質があるからである。

また、このような本実施形態に係る自己位置姿勢標定装置は、同一場所付近の旧画像と新画像を照らし合わせることで、新旧それぞれの画像のカメラ位置と姿勢を三次元的に対応づける技術から成り立っているので、新旧画像の更新装置としても利用することができる。
つまり、本実施形態に係る自己位置姿勢標定装置を用いることにより、車両等の移動体を走行・移動させることで、画像更新を行いながらの自動走行が可能となる。
さらには、画像更新のために自動運転車両等を走行させれば良いことになる。

［ＣＶ映像地図の特徴］
まず、本発明に係るＣＶ映像地図の特徴について説明する。
後述するように（図１〜１３参照）、ＣＶ映像地図内の任意の点は、隣接するフレーム間で、対応点処理することで、いつでも三次元座標を取得できる状態になっている。この方式は、画像内の任意の点を三次元化するための一手間（ＣＶ演算処理）を必要とするが、ＣＶ演算処理はミリセカンドの時間で処理できるので、実質的には、ＣＶ映像は映像内のすべての三次元座標を持っているのと同じ意味となる。したがって、ＣＶ映像を自己位置姿勢標定装置のための基準となる三次元地図として用いることができる。

さらに、ＣＶ演算に使用した多くの特徴点（数十から数百の特徴点）は、演算の途中で三次元座標を持つことになるが、データとして保存されるときには、この数百の特徴点と、その三次元座標はいったん破棄される。そして、破棄されても、カメラ位置と姿勢の６変数があれば、いつでも映像内の任意の点を三次元化することができる。
この三次元化された特徴点の座標データを破棄する処理によって、保存するデータ量を極端に減少させることが可能となる。この点において、移動体の移動範囲の環境全体の三次元点群を、すべて保存した状態で移動・処理等する必要のある従来の方式（特許文献１参照）と大きく異なる、ＣＶ映像の極めて有利な優れた特徴となる。

また、例えば後の何らかの目的のために、必要に応じてＣＶ演算の途中で得られた三次元特徴点の一部（数十点から数百点程度）を破棄せず残したとしても、全体のデータ量を大きく増加させることはない。
このように、本発明に係るＣＶ映像を用いることによって、初めて自動運転の三次元地図を実用的なデータ量とすることが可能となる。
しかも、このようなＣＶ映像は、各フレーム内の画像の任意の点は、データとしては未だ三次元座標を持っていないが、数ミリセカンドの一手間の演算をすることで、任意の点の三次元座標を直ちに取得することができ、三次元地図として利用することができるという優れた特徴がある。

このように、本発明に係るＣＶ映像を三次元地図として用いることにより、自動走行する車両等に設置されたカメラから得られる目的画像の全点ではなく、目的画像内のごく少数の必要な特徴点のみを取り出して、両者画像を比較することで、目的画像の自己位置姿勢標定を行うことが可能となる。
これによって、従来技術のように、三次元地図の作成とそれに基づく自己位置姿勢標定のためのデータが莫大な量となってしまうという問題を生じさせることなく、簡易かつ迅速に、移動体の自動運転に不可欠な自己位置姿勢標定を実現することができるようになる。
さらに、本発明では、移動体に搭載・設置したカメラから得られた画像がそのまま目的画像となるので、従来技術と比較して、費用を大幅に低下・削減することが可能となり、低コストで高精度な自己位置姿勢標定が可能となるという優れた特徴を有するものである。

［用語の定義］
次に、本明細書・特許請求の範囲中において使用する用語・語句についての定義を説明する。
・ＣＶ映像地図：
自己位置姿勢標定装置において参照基準となる三次元地図とは、前もってカメラ映像から取得された映像、又は連続する画像から、当該カメラ位置と姿勢を６変数で求めることで生成されたＣＶ映像を三次元地図として利用するものである。これをＣＶ映像地図という。
したがって、ＣＶ映像地図には、ＣＶ映像、及びＣＶ映像から生成された、あるいは他の方法で生成された三次元ＣＧを含むことができ、さらに、当該ＣＶ映像から画像処理して生成された各種の点・図形等を合体させたものまで含ませることができる。また、特殊な場合として、ＣＶ映像から生成した各種三次元形状のみであっても、元がＣＶ映像であれば、ＣＶ映像地図と呼称することができる。特にこれをＣＶ３Ｄ地図と呼称することができる。
つまり、３Ｄ地図の目的を持つＣＶ映像は、ＣＶ映像地図ということができる。

ここで、本発明に係るＣＶ映像地図は、上述したＳＬＡＭやＶ−Ｓｌａｍのように、環境の全空間の三次元座標を点群として持たない。ＳＬＡＭにしろＶ−Ｓｌａｍにしろ、目的の３Ｄ地図と、目的の自己位置姿勢標定を行うために必要なだけの精度に合った点群（数百万点〜数億点／ｋｍ）を持つことになる。
これに対して、本発明に係るＣＶ映像は、基本的に点群を持たず、いつでも必要なときに、隣接画像から目的の点の三次元座標をその場で自動演算により求めることができるものである。全空間の三次元座標は、カメラ位置（６変数）から演算で求められる。

すなわち、ＣＶ映像地図は、従来のＳＬＡＭやＶ−Ｓｌａｍのように、全環境の三次元座標を持たずに、二次元映像とカメラ位置の６変数を持つことを基本としている。必要な三次元点の座標は、その場で演算により求めることができる。その演算速度は、一点に付き、数ミリセカンド以下である。
一般に、自己位置姿勢標定には、三次元点の座標は、４点以上〜１０点程度で十分であるから、本発明に係るＣＶ演算を用いることで、十分な速度で必要な座標の三次元座標値を求めることができる。そして、このようなＣＶ演算方式は、三次元点群を持たないことから、データが軽く、扱いがし易く、データ伝送についてもＳＬＡＭやＶ−Ｓｌａｍと比較して極端に狭帯域で実現できるようになる。

さらに、ＣＶ映像地図に基づいて得られる目的の車両等の自己位置姿勢標定は６変数であるが、一方で、６変数は機械センサーのＩＭＵ／ＧＹＲＯ／ＧＮＳＳ（ＧＰＳ）によっても取得できる。
そのため、車載されたカメラで得られた画像（目的画像）に、機械センサーであるＩＭＵ／ＧＹＲＯ／ＧＮＳＳ（ＧＰＳ）を組み合わせで得られたものであっても、これをＣＶ映像地図・ＣＶ３Ｄ地図とすることができる。
機械センサー（ＩＭＵ／ＧＹＲＯ／ＧＮＳＳ）は、演算遅れや、時間不連続期間などの極微少時間であれば、低価格のＩＭＵ／ＧＹＲＯ／ＧＮＳＳであっても、目的カメラの補正機能として十分に有効である。機械センサーで取得された６変数は、本質的にＣＶ値で示される６変数と同等である。
したがって、ＩＭＵ／ＧＹＲＯ／ＧＮＳＳ等を取り込んだ地図もＣＶ映像地図に含むことができる。

・ＣＶ値／ＣＶ映像／ＣＶ画像：
移動する物体、（たとえば車両やロボット等）に積載された機械センサーで得られた６変数、及びカメラから得られた連続する静止画像、又は動画像から、カメラの位置と姿勢を算出した６変数をカメラベクトル（ＣＶ）と呼称し、その値をＣＶ値といい、ＣＶ値を演算で求めることをＣＶ演算といい、そしてそのようなＣＶ値を有する画像をＣＶ映像という。
なお、カメラに一体化された機械センサーから得られた６変数も、本発明に係るＣＶ値とすることができる。
また、連続しない単独の画像についてはＣＶ画像と呼称することがある。あるいは、１フレームについてのみ着目するときには、当該１フレームの画像をＣＶ画像と呼称することがある。つまり、ＣＶ画像はＣＶ映像の特殊な状態（単独画像）である。

・ＣＶ機械地図：
ＣＶ３Ｄ地図は、人間ではなく、コンピュータを利用して自動で読み取ることが可能である。この場合には、ＣＶ機械地図と呼称することがある。
また、このようなＣＶ機械地図は、ＣＶ３Ｄ地図からコンピュータに読み取り易く処理することが可能であり、これもＣＶ機械地図と呼称することがある。
さらに、データベース化されたＣＶ機械地図をＣＶ機械地図ＤＢと呼称する。
これらはいずれも、本発明に係るＣＶ映像地図である。
なお、上記のような「ＣＶ映像地図／ＣＶ機械地図」の用語は、基本的に明確な区別はなく、本明細書中においては、以下用途によって語句を使い分ける。

・目的移動体／目的カメラ／目的画像：
本発明を適用して自己位置姿勢標定を行う対象の移動体、すなわち、自動運転の対象となる移動体を目的移動体という。例えば、自動走行する車両、ロボットなどである。
目的移動体に積載されたカメラを目的カメラ、目的カメラによって取得された画像を目的画像という。

［ＣＶ演算］
次に、上記のような本発明の基準映像地図を用いた自己位置姿勢標定装置で用いられるＣＶ映像地図におけるＣＶ演算の詳細について図１〜図１３を参照しつつ説明する。
ＣＶ演算とはＣＶ値を求めることを意味し、求められた結果をＣＶ値，ＣＶデータと呼ぶ。ＣＶという表記は、カメラベクトル：Camera Vectorの略記であり、カメラベクトル（ＣＶ）とは計測等のために映像を取得するビデオカメラ等のカメラの三次元位置と３軸回転姿勢を示す値である。
ＣＶ演算は、動画像（ビデオ映像）を取得し、その映像内の特徴点を検出し、それを隣接する複数のフレームに追跡し、カメラ位置と特徴点の追跡軌跡とが作る三角形を画像内に数多く生成し、その三角形を解析することで、カメラの三次元位置とカメラの３軸回転姿勢を求めるものである。

ＣＶ演算では、ＣＶ値を求める過程で、同時に映像内の特徴点（基準点）についても三次元座標が同時に求まることが重要な特性である。
また、動画像から演算で求められるＣＶ値は、動画像の各フレームに対応して、三次元のカメラ位置と三次元のカメラ姿勢とが同時に求まる。しかも、原理的には一台のカメラで、映像と対応してＣＶ値が求められる特性は、ＣＶ演算でしか実現し得ない、優れた特徴である。
例えば、他の方法による計測手段（ＧＰＳやＩＭＵ等）では、動画像の各フレームと、その三次元的カメラ位置と三次元的カメラ姿勢とを同時に取得するためには画像フレームと計測サンプリング時刻を高精度で、しかも完全に同期しなければならないために、巨額の装置となり、実質的には実現が困難である。

動画像から演算で求められるＣＶデータは、加工しない段階では相対値であるが、短区間であれば高精度で三次元位置情報と３軸回転の角度情報を取得できる。
また、ＣＶデータは画像から取得するため、取得されたデータは相対値であるが、画像内の任意の対象物との位置関係を計測することができるという他の方法では実現は可能な優れた特性を備える。
また、画像に対応したＣＶ値が求まるので、画像内計測や測量において、画像から直接にカメラ位置とその３軸回転姿勢を求めることができるＣＶ演算は画像内計測や画像内測量に好適となる。
そして、本発明の基準映像地図を用いた自己位置姿勢標定装置は、このＣＶ演算により得られたＣＶ値データに基づいて基準映像と目的映像（比較映像）との座標統合処理（ＣＶ値の移植・移転）を行うものである。

［ＣＶ演算手段］
ＣＶ演算は、後述する本発明の基準映像地図を用いた自己位置姿勢標定装置のＣＶ映像地図作成装置２０（図１４参照）として機能するＣＶ演算手段２０で行われる。
ＣＶ演算手段（ＣＶ映像地図作成装置）２０は、図１に示すように、車載のビデオカメラ等で構成されるＣＶ映像取得装置１０から入力されるビデオ映像について所定のＣＶ演算処理を行うようになっており、具体的には、特徴点抽出部２１と、特徴点対応処理部２２と、カメラベクトル演算部２３と、誤差最小化部２４と、三次元情報追跡部２５と、高精度カメラベクトル演算部２６とを備えている。

まず、ＣＶ演算に使用する映像としては、どのような映像でもよいが、画角の限られた映像では視点方向を移動した場合に映像がとぎれてしまうので、全周映像（図２〜４参照）とすることが望ましい。なお、動画映像は連続する静止画と同様であり、静止画と同様に扱うことができる。
また、映像は、一般には予め記録した動画映像を使うことになるが、自動車等の移動体の移動に合わせてリアルタイムに取り込んだ映像を使用することも勿論可能である。

そこで、本実施形態では、ＣＶ演算に使用する映像として、車輌等の移動体の３６０度の全周囲を撮影した全周映像（図２〜４参照）か、又は全周映像に近い広角映像を用いて、その全周映像を視点方向に平面展開することにより、地図と映像の合成画像を生成・表示するＣＶ映像取得装置１０を備えている（図１参照）。
ここで、全周映像の平面展開とは、全周映像を、通常の画像として遠近法的に表現するものである。ここで、「遠近法」と呼称するのは、全周画像のそのものはメルカトール図法や球面投影図法のように、遠近法とは異なる方法で表示されているので（図４参照）、これを平面展開表示することで、通常の遠近法映像に変換表示できるからである。

ＣＶ映像取得装置１０において全周映像を生成するには、まず、図２及び図３に示すように、全周ビデオカメラ１１を使用して、ＣＶ値データを取得する目的で、走行車輌等の移動体１１ａに固定された全周ビデオカメラ１１で、移動体１１ａの移動とともに移動体周辺を撮影する。
なお、移動体１１ａには、その位置座標を取得する目的で、例えば、絶対座標を取得するＧＰＳ機器単独やＩＭＵ機器を付加したもの等により構成した位置計測機器等を備えることができる。
また、移動体１１ａに搭載される全周ビデオカメラ１１としては、広範囲映像を撮影，取得するカメラであればどのような構成であってもよく、例えば、広角レンズや魚眼レンズ付きカメラ、移動カメラ、固定カメラ、複数のカメラを固定したカメラ、３６０度周囲に回転可能なカメラ等がある。本実施形態では、図２及び図３に示すように、車輌に複数のカメラが一体的に固定され、移動体１１ａの移動に伴って広範囲映像を撮影する全周ビデオカメラ１１を使用している。

そして、以上のような全周ビデオカメラ１１によれば、図３に示すように、移動体１１ａの天井部に設置されることで、カメラの３６０度全周囲の映像を複数のカメラで同時に撮影することができ、移動体１１ａが移動することで、広範囲映像を動画データとして取得できる。
ここで、全周ビデオカメラ１１は、カメラの全周映像を直接取得できるビデオカメラであるが、カメラの全周囲の半分以上を映像として取得できれば全周映像として使用できる。
また、画角が制限された通常のカメラの場合でも、ＣＶ演算の精度としては低下するが、全周映像の一部分として取り扱うことが可能である。

なお、全周ビデオカメラ１１で撮影された広範囲映像は、一枚の画像として、撮影時の画角に一致する仮想球面に貼り付けることができる。
仮想球面に貼り付けられた球面画像データは、仮想球面に貼り付けた状態の球面画像（３６０度画像）データとして保存・出力される。仮想球面は、広範囲映像を取得するカメラ部を中心点とした任意の球面状に設定することができる。
図４（ａ）は球面画像が貼り付けられる仮想球面の外観イメージであり、同図（ｂ）は仮想球面に貼り付けられた球面画像の一例である。また、同図（ｃ）は、（ｂ）の球面画像をメルカトール図法に従って平面展開した画像例を示す。

そして、以上のように生成・取得された全周ビデオ映像が、ＣＶ演算手段（ＣＶ映像地図作成装置）２０に入力されてＣＶ値データが求められる（図１参照）。
ＣＶ演算手段２０では、まず、特徴点抽出部２１が、ＣＶ映像取得装置１０の全周ビデオカメラ１１で撮影されて一時記録された動画像データの中から、十分な数の特徴点（基準点）を自動抽出する。
特徴点対応処理部２２は、自動抽出された特徴点を、各フレーム間で各フレーム画像内において自動的に追跡することで、その対応関係を自動的に求める。
カメラベクトル演算部２３は、対応関係が求められた特徴点の三次元位置座標から各フレーム画像に対応したカメラベクトルを演算で自動的に求める。
誤差最小化部２４は、複数のカメラ位置の重複演算により、各カメラベクトルの解の分布が最小になるように統計処理し、誤差の最小化処理を施したカメラ位置方向を自動的に決定する。

三次元情報追跡部２５は、カメラベクトル演算部２３で得られたカメラベクトルを概略のカメラベクトルと位置づけ、その後のプロセスで順次画像の一部として得られる三次元情報に基づいて、複数のフレーム画像に含まれる部分的三次元情報を隣接するフレームの画像に沿って自動追跡を行う。ここで、三次元情報（三次元形状）とは、主に特徴点の三次元分布情報であり、すなわち、三次元の点の集まりであり、この三次元の点の集まりが三次元形状を構成する。
高精度カメラベクトル演算部２６は、三次元情報追跡部２５で得られた追跡データに基づいて、カメラベクトル演算部２３で得られるカメラベクトルより、さらに高精度なカメラベクトルを生成，出力する。
そして、以上のようにして得られたカメラベクトルが、後述する基準映像地図を用いた自己位置姿勢標定装置１０に入力され、基準映像と目的画像の座標統合処理（ＣＶ値の移転・統合）に利用されることになる。

複数の画像（動画又は連続静止画）の特徴点からカメラベクトルを検出するには幾つかの方法があるが、図１に示す本実施形態のＣＶ演算手段２０では、画像内に十分に多くの数の特徴点を自動抽出し、それを自動追跡することで、エピポーラ幾何学により、カメラの三次元ベクトル及び３軸回転ベクトルを求めるようにしてある。
特徴点を充分に多くとることにより、カメラベクトル情報が重複することになり、重複する情報から誤差を最小化させて、より精度の高いカメラベクトルを求めることができる。

カメラベクトルとは、カメラの持つ自由度のベクトルである。
一般に、静止した三次元物体は、位置座標（Ｘ，Ｙ，Ｚ）と、それぞれの座標軸の回転角（Φｘ，Φｙ，Φｚ）の六個の自由度を持つ。
従って、カメラベクトルは、カメラの位置座標（Ｘ，Ｙ，Ｚ）とそれぞれの座標軸の回転角（Φｘ，Φｙ，Φｚ）の六個の自由度のベクトル（６変数）をいう。なお、カメラが移動する場合は、自由度に移動方向も入るが、これは上記の六個の自由度（変数）から微分して導き出すことができる。
このように、本実施形態のカメラベクトルの検出とは、カメラは各フレーム毎に六個の自由度の値をとり、各フレーム毎に異なる六個の自由度を決定することである。

以下、ＣＶ演算手段２０における具体的なカメラベクトルの検出方法について、図５以下を参照しつつ説明する。
まず、上述したＣＶ映像取得装置１０の全周ビデオカメラ１１で取得された画像データは、間接に又は直接に、ＣＶ演算手段２０の特徴点抽出部２１に入力され、特徴点抽出部２１で、適切にサンプリングされたフレーム画像中に、特徴点となるべき点又は小領域画像が自動抽出され、特徴点対応処理部２２で、複数のフレーム画像間で特徴点の対応関係が自動的に求められる。
具体的には、カメラベクトルの検出の基準となる、十分に必要な数以上の特徴点を求める。画像間の特徴点とその対応関係の一例を、図５〜図７に示す。図中「＋」が自動抽出された特徴点であり、複数のフレーム画像間で対応関係が自動追跡される（図７に示す対応点１〜４参照）。
ここで、特徴点の抽出は、図８に示すように、各画像中に充分に多くの特徴点を指定，抽出することが望ましく（図８の○印参照）、例えば、１００点程度の特徴点を抽出する。

続いて、カメラベクトル演算部２３で、抽出された特徴点の三次元座標が演算により求められ、その三次元座標に基づいてカメラベクトルが演算により求められる。具体的には、カメラベクトル演算部２３は、連続する各フレーム間に存在する、十分な数の特徴の位置と、移動するカメラ間の位置ベクトル、カメラの３軸回転ベクトル、各カメラ位置と特徴点をそれぞれ結んだベクトル等、各種三次元ベクトルの相対値を演算により連続的に算出する。
本実施形態では、例えば、３６０度全周画像のエピポーラ幾何からエピポーラ方程式を解くことによりカメラ運動（カメラ位置とカメラ回転）を計算するようになっている。

図７に示す画像１，２は、３６０度全周画像をメルカトール展開した画像であり、緯度φ、軽度θとすると、画像１上の点は（θ１，φ１）、画像２上の点は（θ２，φ２）となる。そして、それぞれのカメラでの空間座標は、ｚ１＝（ｃｏｓφ１ｃｏｓθ１，ｃｏｓφ１ｓｉｎθ１，ｓｉｎφ１）、ｚ２＝（ｃｏｓφ２ｃｏｓθ２，ｃｏｓφ２ｓｉｎθ２，ｓｉｎφ２）である。カメラの移動ベクトルをｔ、カメラの回転行列をＲ、とすると、ｚ１^T［ｔ］×Ｒｚ２＝０がエピポーラ方程式である。
十分な数の特徴点を与えることにより、線形代数演算により最小自乗法による解としてｔ及びＲを計算することができる。この演算を対応する複数フレームに適用し演算する。

ここで、カメラベクトルの演算に利用する画像としては、３６０度全周画像を用いることが好ましい。
カメラベクトル演算に用いる画像としては、原理的にはどのような画像でも良いが、図７に示す３６０度全周画像のような広角画像の方が特徴点を数多く選択し易くなる。そこで、本実施形態では、ＣＶ演算に３６０度全周画像を用いており、これによって、特徴点の追跡距離を長くでき、特徴点を十分に多く選択することができ、遠距離、中距離、短距離それぞれに都合の良い特徴点を選択することができるようになる。また、回転ベクトルを補正する場合には、極回転変換処理を加えることで、演算処理も容易に行えるようになる。これらのことから、より精度の高い演算結果が得られるようになる。
なお、図７は、ＣＶ演算手段２０における処理を理解し易くするために、１台又は複数台のカメラで撮影した画像を合成した３６０度全周囲の球面画像を地図図法でいうメルカトール図法で展開したものを示しているが、実際のＣＶ演算では、必ずしもメルカトール図法による展開画像である必要はない。

次に、誤差最小化部２４では、各フレームに対応する複数のカメラ位置と複数の特徴点の数により、複数通り生じる演算方程式により、各特徴点に基づくベクトルを複数通り演算して求めて、各特徴点の位置及びカメラ位置の分布が最小になるように統計処理をして、最終的なベクトルを求める。例えば、複数フレームのカメラ位置、カメラ回転及び複数の特徴点について、Levenberg-Marquardt法により最小自乗法の最適解を推定し、誤差を収束してカメラ位置、カメラ回転行列、特徴点の座標を求める。
さらに、誤差の分布が大きい特徴点につては削除し、他の特徴点に基づいて再演算することで、各特徴点及びカメラ位置での演算の精度を上げるようにする。
このようにして、特徴点の位置とカメラベクトルを精度良く求めることができる。

図９〜図１１に、ＣＶ演算により得られる特徴点の三次元座標とカメラベクトルの例を示す。図９〜図１１は、本実施形態のＣＶ演算によるベクトル検出方法を示す説明図であり、移動するカメラによって取得された複数のフレーム画像によって得られるカメラ及び対象物の相対的な位置関係を示す図である。
図９では、図７の画像１，２に示した特徴点１〜４の三次元座標と、画像１と画像２の間で移動するカメラベクトル（Ｘ，Ｙ，Ｚ）が示されている。
図１０及び図１１は、充分に多くの特徴点とフレーム画像により得られた特徴点の位置と移動するカメラの位置が示されている。同図中、グラフ中央に直線状に連続する○印がカメラ位置であり、その周囲に位置する○印が特徴点の位置と高さを示している。

ここで、ＣＶ演算手段２０におけるＣＶ演算は、より高精度な特徴点とカメラ位置の三次元情報を高速に得るために、図１２に示すように、カメラから特徴点の距離に応じて複数の特徴点を設定し、複数の演算を繰り返し行うようにする。
具体的には、ＣＶ演算手段２０では、画像内には映像的に特徴がある特徴点を自動検出し、各フレーム画像内に特徴点の対応点を求める際に、カメラベクトル演算に用いるｎ番目とｎ＋ｍ番目の二つのフレーム画像ＦｎとＦｎ＋ｍに着目して単位演算とし、ｎとｍを適切に設定した単位演算を繰り返すことができる。
ｍはフレーム間隔であり、カメラから画像内の特徴点までの距離によって特徴点を複数段に分類し、カメラから特徴点までの距離が遠いほどｍが大きくなるように設定し、カメラから特徴点までの距離が近いほどｍが小さくなるように設定する。このようにするのは、カメラから特徴点までの距離が遠ければ遠いほど、画像間における位置の変化が少ないからである。

そして、特徴点のｍ値による分類を、十分にオーバーラップさせながら、複数段階のｍを設定し、画像の進行とともにｎが連続的に進行するのにともなって、演算を連続的に進行させる。そして、ｎの進行とｍの各段階で、同一特徴点について複数回重複演算を行う。
このようにして、フレーム画像ＦｎとＦｎ＋ｍに着目した単位演算を行うことにより、ｍ枚毎にサンプリングした各フレーム間（フレーム間は駒落ちしている）では、長時間かけて精密カメラベクトルを演算し、フレーム画像ＦｎとＦｎ＋ｍの間のｍ枚のフレーム（最小単位フレーム）では、短時間処理で行える簡易演算とすることができる。

ｍ枚毎の精密カメラベクトル演算に誤差がないとすれば、ｍ枚のフレームのカメラベクトルの両端は、高精度演算をしたＦｎとＦｎ＋ｍのカメラベクトルと重なることになる。従って、ＦｎとＦｎ＋ｍの中間のｍ枚の最小単位のフレームについては簡易演算で求め、簡易演算で求めたｍ枚の最小単位フレームのカメラベクトルの両端を、高精度演算で求めたＦｎとＦｎ＋ｍのカメラベクトルに一致するように、ｍ枚の連続したカメラベクトルのスケール調整をすることができる。
このようにして、画像の進行とともにｎが連続的に進行することにより、同一特徴点について複数回演算されて得られる各カメラベクトルの誤差が最小になるようにスケール調整して統合し、最終のカメラベクトルを決定することができる。
これにより、誤差のない高精度のカメラベクトルを求めつつ、簡易演算を組み合わせることにより、演算処理を高速化することができるようになる。

ここで、簡易演算としては、精度に応じて種々の方法があるが、例えば、(1)高精度演算では１００個以上の多くの特徴点を用いる場合に、簡易演算では最低限の１０個程度の特徴点を用いる方法や、(2)同じ特徴点の数としても、特徴点とカメラ位置を同等に考えれば、そこには無数の三角形が成立し、その数だけの方程式が成立するため、その方程式の数を減らすことで、簡易演算とすることができる。
これによって、各特徴点及びカメラ位置の誤差が最小になるようにスケール調整する形で統合し、距離演算を行い、さらに、誤差の分布が大きい特徴点を削除し、必要に応じて他の特徴点について再演算することで、各特徴点及びカメラ位置での演算の精度を上げることができる。

また、このように高速な簡易演算を行うことにより、カメラベクトルのリアルタイムに近い処理が可能となる。カメラベクトルの高速演算処理は、目的の精度をとれる最低のフレーム数と、自動抽出した最低の特徴点数で演算を行い、カメラベクトルの概略値を高速演算で求め、表示し、次に、画像が蓄積するにつれて、フレーム数を増加させ、特徴点の数を増加させ、より精度の高いカメラベクトル演算を行い、概略値を精度の高いカメラベクトル値に置き換えて表示することができる。

さらに、本実施形態では、より高精度のカメラベクトルを求めるために、三次元情報（三次元形状）の追跡を行うことができる。
具体的には、まず、三次元情報追跡部２５で、カメラベクトル演算部２３，誤差最小化部２４を経て得られたカメラベクトルを概略のカメラベクトルと位置づけ、その後のプロセスで生成される画像の一部として得られる三次元情報（三次元形状）に基づいて、複数のフレーム画像に含まれる部分的三次元情報を隣接するフレーム間で連続的に追跡して三次元形状の自動追跡を行う。
そして、この三次元情報追跡部２５で得られた三次元情報の追跡結果から、高精度カメラベクトル演算部２６においてより高精度なカメラベクトルが求められる。

上述した特徴点抽出部２１及び特徴点対応処理部２２では、特徴点を複数のフレーム間画像内に自動追跡するが、特徴点が消失するなどして特徴点の追跡フレーム数に制限が出てくることがある。また、画像は二次元であり、追跡途中で形状が変化するために追跡精度にも一定の限界がある。
そこで、特徴点追跡で得られるカメラベクトルを概略値と位置づけ、その後のプロセスで得られる三次元情報（三次元形状）を各フレーム画像上に追跡して、その軌跡から高精度カメラベクトルを求めることができる。
三次元形状の追跡は、マッチング及び相関の精度を得やすく、三次元形状はフレーム画像によって、その三次元形状も大きさも変化しないので、多くのフレームに亘って追跡が可能であり、そのことでカメラベクトル演算の精度を向上させることができる。これはカメラベクトル演算部２３により概略のカメラベクトルが既知であり、三次元形状が既に分かっているから可能となるものである。

カメラベクトルが概略値の場合、非常に多くのフレームに亘る三次元座標の誤差は、特徴点追跡による各フレームに関係するフレームが少ないので、誤差が累積して長距離では次第に大きな誤差になるが、画像の一部分を切り取ったときの三次元形状の誤差は相対的に少なく、形状の変化と大きさに及ぼす影響はかなり少ないものとなる。このため、三次元形状での比較や追跡は、二次元形状追跡の時よりも極めて有利となる。追跡において、二次元形状での追跡の場合、複数のフレームにおける形状の変化と大きさの変化を避けられないまま追跡することになるので、誤差が大きかったり、対応点が見つからないなどの問題があったが、三次元形状での追跡においては形状の変化が極めて少なく、しかも原理的に大きさの変化もないので、正確な追跡が可能となる。

ここで、追跡の対象となる三次元形状データとしては、例えば、特徴点の三次元分布形状や、特徴点の三次元分布形状から求められるポリゴン面等がある。
また、得られた三次元形状を、カメラ位置から二次元画像に変換して、二次元画像として追跡することも可能である。カメラベクトルの概略値が既知であることから、カメラ視点からの二次元画像に投影変換が可能であり、カメラ視点の移動による対象の形状変化にも追従することが可能となる。

以上のようにして求められたカメラベクトルは、全周ビデオカメラ１１で撮影されたビデオ映像中に重ねて表示することができる。
例えば、図１３に示すように、車載カメラからの映像を平面展開して、各フレーム画像内の目的平面上の対応点を自動で探索し、対応点を一致させるように結合して目的平面の結合画像を生成し、同一の座標系に統合して表示する。
さらに、その共通座標系の中にカメラ位置とカメラ方向を次々に検出し、その位置や方向、軌跡をプロットしていくことができる。ＣＶデータは、その三次元位置と３軸回転を示しており、ビデオ映像に重ねて表示することで、ビデオ映像の各フレームでＣＶ値を同時に観察できる。ＣＶデータをビデオ映像に重ねた表示した画像例を図１３に示す。
なお、ビデオ映像内にカメラ位置を正しく表示すると、ＣＶ値が示すビデオ映像内の位置は画像の中心となり、カメラ移動が直線に近い場合は、すべてのフレームのＣＶ値が重なって表示されてしまうので、例えば図１３に示すように、敢えてカメラ位置から真下に１メートルの位置を表示することが適切である。あるいは道路面までの距離を基準として、道路面の高さにＣＶ値を表示するのがより適切である。

［自己位置姿勢標定装置］
次に、以上のようにして求められたＣＶ値に基づいて基準映像（ＣＶ映像）と、それに比較される目的画像の座標統合処理を行う本発明に係る基準映像地図（ＣＶ映像地図）を用いた自己位置姿勢標定装置の実施形態について、図面を参照しつつ具体的に説明する。
なお、以下に示す基準映像地図（ＣＶ映像地図）を用いた自己位置姿勢標定装置において、基準映像・目的画像と表現する場合、必ずしも目的画像が時間的・時刻的に新しい映像であり、基準映像が旧い映像であるという意味ではない。
例えば、画像更新装置を目的とした自己位置姿勢標定装置の場合には、ＣＶ値が既知の映像が、基準映像となるものであり、基準映像に基づいてＣＶ値が取得（移植・統合）される映像が、目的画像（目的映像）となるものである。

図１４は、本発明の一実施形態に係る基準映像地図（ＣＶ映像地図）を用いた自己位置姿勢標定装置１の基本構成を示すブロック図である。
図１４に示すように、本実施形態に係る基準映像地図（ＣＶ映像地図）を用いた自己位置姿勢標定装置１は、上述したＣＶ映像地図を基準映像として、移動する車両等の移動体が自らの位置と姿勢をリアルタイムに標定し、その自己位置姿勢標定に基づいて移動体の自動運転等を実現するための装置・手段である。
具体的には、本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置１は、ＣＶ映像取得装置１０と、ＣＶ映像地図作成装置２０と、ＣＶ映像地図データベース（ＣＶ機械地図データベース）３０と、目的移動体（自動運転装置）４０と、ＣＶ映像地図・目的画像比較装置５０と、自己位置姿勢標定装置６０を備えている。

ＣＶ映像取得装置１０は、自己位置姿勢標定の基準映像地図を生成するための基準映像を撮影・取得するための手段であり、上述した図１〜３に示すように、全周ビデオカメラ１１を備えた走行車両等の移動体１１ａによって構成される。
この移動体１１ａが、基準映像地図を取得する目的で、所定の道路等を一定範囲で走行することで、移動体１１ａに備えられた全周ビデオカメラ１１により、移動体１１ａの移動に伴って移動体周辺の映像を基準映像として撮影・取得する。
このＣＶ映像取得装置１０で取得された基準映像が、ＣＶ映像地図作成装置２０に入力されて、上述したＣＶ演算に基づくＣＶ映像地図の作成処理が行われる（図１〜１３参照）。

ＣＶ映像地図作成装置２０は、所定の映像取得手段で撮影された基準映像に基づいて、当該基準映像のカメラ位置と姿勢の三次元座標値を示すＣＶ（カメラベクトル）値を求めるＣＶ演算を行い、基準映像にＣＶ値を付加したＣＶ映像地図を生成する手段であり、本願請求項１のＣＶ映像地図作成手段を構成している。
具体的には、ＣＶ映像地図作成装置２０は、上述した図１〜１３で示したＣＶ演算手段によって構成される。ＣＶ映像地図作成装置２０によるＣＶ演算の具体的な内容については、上述したとおりである（図１〜１３参照）。

ＣＶ映像地図データベース（ＣＶ機械地図データベース）３０は、ＣＶ映像地図作成装置２０で生成されたＣＶ映像地図を記憶する記憶手段であり、本願請求項１のＣＶ映像地図データベースを構成している。
このＣＶ映像地図データベース３０に記憶されたＣＶ映像地図が、自己位置姿勢標定処理のための基準映像となる三次元地図データとして記憶・保持され、自己位置姿勢標定装置６０により読み出されて、所定の目的画像との比較参照・座標統合等が行われることになる。

目的移動体（自動運転装置）４０は、自動運転の対象となる車両等で構成され、本実施形態の自己位置姿勢標定の目的となる移動物体である。
この目的移動体４０には、自己位置姿勢標定の目的画像となる画像・映像を撮影・取得するための手段として、上述したＣＶ映像取得装置１０と同様に、例えばビデオカメラや車載カメラ等の撮像手段（目的カメラ）が備えられている。
また、目的移動体４０には、当該移動体の位置情報を取得する手段として、上述のようなＩＭＵ／ＧＹＲＯ／ＧＮＳＳ（ＧＰＳ）などの記載センサーが備えられている。

そして、カメラ及び機械センサー等を備えた目的移動体４０が、例えば自動運転の対象範囲となる所定の道路等を走行することで、移動体に備えられたカメラ等により、移動体の移動に伴って移動体周辺の映像が目的画像として撮影・取得され、その目的画像の三次元位置情報が機械センサーによって取得・付与される。
この目的移動体４０で取得された目的画像及び機械センサーで得られた６変数データが、ＣＶ映像地図・目的画像比較装置５０を介して自己位置姿勢標定装置６０に入力されて、上述した基準映像となるＣＶ映像地図と対比・参照されて、目的画像に対して三次元座標となるＣＶ値が移植・統合・補正されることになる。

ＣＶ映像地図・目的画像比較装置５０は、目的移動体４０に備えられた所定の画像取得手段で撮影された目的画像と、当該目的画像に対応する機械センサーにより取得された６変数データを入力・受信する。そして、当該目的画像を対比・参照させるべき基準画像となるＣＶ映像地図をＣＶ映像地図データベース３０から読み出し、これら目的画像・機械センサーで得られる６変数データ・ＣＶ映像地図を自己位置姿勢標定装置６０に出力する。

ここで、目的画像と比較すべき基準映像となるＣＶ映像地図は、例えば、ＣＶ映像地図に付与されている概略位置情報等に基づき、目的画像に対応するＣＶ映像地図が読み出される。なお、この場合、目的画像には、例えばＧＰＳ等により概略位置情報が付与されているとする。
これにより、自己位置姿勢標定装置６０では、目的画像・機械センサーで得られる６変数・ＣＶ映像地図の各データに基づき、自己位置姿勢標定処理が実行されるようになる。

自己位置姿勢標定装置６０は、ＣＶ映像地図データベース３０に記憶されたＣＶ映像地図を基準画像とし、目的移動体４０に備えられた所定の画像取得手段で撮影された目的画像をＣＶ映像地図と比較して、当該目的画像とＣＶ映像地図の同一箇所を示す複数の特徴点を自動的に対応させることにより、当該目的画像のＣＶ値を取得する手段であり、本願請求項１の自己位置姿勢標定手段を構成している。
まず、自己位置姿勢標定装置６０には、ＣＶ映像地図・目的画像比較装置５０を介して、目的移動体４０の撮像手段（目的カメラ）で取得される目的画像と、機械センサーで得られる目的移動体４０の位置情報となる６変数が入力される。

また、自己位置姿勢標定装置６０には、ＣＶ映像地図データベース３０に記憶されたＣＶ映像地図のうち、目的画像と比較・参照すべき大まかな範囲を基準画像として読み込み、ＣＶ映像地図・目的画像比較装置５０を介して読み出されて入力される。
そして、自己位置姿勢標定装置６０は、入力された目的画像と機械センサーで得られた６変数データとＣＶ映像地図に基づいて、目的画像とＣＶ映像地図の同一箇所を示す複数の特徴点を自動的に対応させることにより、ＣＶ映像地図に付加されたＣＶ値を、対応する目的画像の特徴点に移植するとで、座標を統合する。
これにより、目的画像にＣＶ値が付与され、目的画像は高精度なＣＶ値を有するＣＶ映像（目的ＣＶ値）として生成・保持されることになる。

具体的には、基準映像となるＣＶ映像地図と目的画像間のＣＶ値の移植・統合処理は、例えば、以下のようにして行われる。
まず、目的画像と基準映像との中の共通地点の対応関係を初期設定する。初期設定されたフレームから開始する動画像の各フレームに亘って、基準映像の中の所定の三次元基準点、又は三次元特徴点、又は二次元特徴点に対応する部分を、目的画像の中に自動的に対応付け、各フレームに亘って対応特徴点、又は対応基準点を探索する。
あるいは、目的画像の中の三次元基準点、又は三次元特徴点、又は二次元特徴点を基準映像の中に探索し、基準映像の各フレームに亘って追跡する。
さらに、目的画像が複数フレームに亘る複数画像や動画映像の場合には、探索処理により対応付けられた対応基準点を、目的画像の進行する各フレームに亘って追跡する。同様に基準映像が複数フレームに亘る複数画像や動画映像の場合には、探索処理により対応付けられた対応基準点を、基準映像の進行する各フレームに亘って追跡する。
そして、対応基準点（対応特徴点も同様）の対応結果により、基準映像の三次元基準点の三次元座標が、目的画像の対応基準点に移植される。

さらに、その移植された対応基準点の三次元座標から目的画像のＣＶ値を、上述したＣＶ演算によって求めることができる。目的画像が複数フレームに亘る場合には、追跡により各フレームに亘ってＣＶ値を求めることができる。
すなわち、基準映像と目的画像との対応が付くことで、基準映像の基準点の三次元座標が目的画像に移植されたことになり、それが四点以上有れば、目的画像のカメラ位置と姿勢がＣＶ演算により求めることができ、目的画像のＣＶ値を取得することができるようになる。
また、このようにＣＶ値が付加された基準映像と、ＣＶ値が求められた目的画像とは、同一の座標系に座標統合されたことになる。

このように、自己位置姿勢標定装置６０では、ＣＶ映像地図を基準として、目的画像と比較をするが、逆に、目的画像から自動取得した既知の三次元点を基準として、ＣＶ映像地図側に対応点を求めて、目的画像のＣＶ値を求めることもできる。
これは自動認識により、目的画像側でのみ、三次元座標を取得する場合に相当する。つまり、目的画像の三次元座標をその場で計測することではなく、例えば、後述する実世界３Ｄマーカ（図１５参照）のように、その目的物の三次元形状と三次元座標が公表されているような場合に、目的対象物を取得することで、公表されている対象物の三次元座標を外から取得できる場合などに利用できる。

また、そのような目的対象物が時間経過等によって移動するような場合にも、目的画像側から、三次元座標を取得することがあり得る。
対象物とは、常に地球上に完全固定されたものは少なく、例えば地震でも移動することがあり、標識などは風雪で傾くこともある。したがって、常に最新の情報を取得することが必要であり、そのために目的画像側から三次元座標を取得することが有用となる。
そして、本実施形態に係る自己位置標定装置６０では、ＣＶ映像地図と目的画像の組み合わせにより、ＣＶ映像地図から目的画像へ、又は／同時に、目的画像からＣＶ映像地図へ、三次元特徴点の三次元座標移転を行うことで、目的画像のＣＶ値取得を行うことができるものである。

さらに、上述のとおり、本発明に係るＣＶ演算は、動画像や連続する静止画像を演算して求めるものであるが、ＣＶ映像地図と目的画像の双方を含めてＣＶ演算を行うことも可能である。
これによって、既知のＣＶ値のみを固定して演算を行い、目的画像側の未知のＣＶ値を求めることで、単独で演算するよりも、単に対応点処理で求めるよりも、高精度なＣＶ値を求めることが可能となる。

具体的には、自己位置標定装置６０では、ＣＶ映像地図と目的画像の組み合わせで、両者の二次元特徴点（２Ｄ）と三次元特徴点（３Ｄ）が混在する中で、両者を一体としてＣＶ演算を行い、そのときに、三次元座標が既知の特徴点（３Ｄ）の三次元座標は固定したまま、両者の全ての特徴点を使ってＣＶ演算を行うことができる。その結果、同時にＣＶ映像地図と目的画像が混在したすべての特徴点が三次元座標を持つことになり、次に目的画像が取得したＣＶ値を新たなＣＶ値として扱うことで、混在するＣＶ値の中から、目的画像のＣＶ値を分離して、自己位置姿勢を標定することが可能となる。

以上のような、自己位置姿勢標定装置６０における自己位置姿勢標定処理の詳細については、図１５〜１７を参照しつつ後述する。
そして、上記のような自己位置姿勢標定装置６０は、目的画像とＣＶ映像地図の同一箇所を示す特徴点として、ＣＶ映像地図に含まれる所定の特徴量として、後述する７種類の特徴量を選択するようになっている（図１５，１６参照）。
この７種類の特徴量については、図１５，１６を参照しつつ後述する。

さらに、自己位置姿勢標定装置６０は、目的移動体４０に備えられた機械センサーで取得される、当該目的移動体４０の自己位置と姿勢を示す６変数データを、目的画像のＣＶ値に基づいて補正することにより、時間的に連続した目的移動体４０の自己位置と姿勢を示す６変数を取得するようになっている。
これによって、目的画像のＣＶ値を、機械センサーで得られる６変数データによって補正・補完することができる（図１７参照）。
この機械センサーによるＣＶ値の補正・補完処理については、図１７を参照しつつ後述する。

そして、以上のようにＣＶ映像地図に基づく自己位置姿勢標定が行われることにより、生成・出力された自己位置姿勢標定結果に基づいて、例えば車両等の移動体の自動運転制御が可能となる。
例えば、図１４の破線で示すように、自動運転の対象となる車両等の目的移動体４０は、自動運転手段によって制御・駆動される。
具体的には、目的移動体４０は、各種センサー等で構成される車両周囲状況判断装置７０の出力信号と、それに基づく走行・停止・回転等の車両の動作を制御する車両制御信号発生装置８０の出力信号により、自動運転が行われる。

このような目的移動体４０の自動運転手段に対して、上述した自己位置姿勢標定装置６０で生成された自己位置姿勢標定情報が入力されることで、ＣＶ値に基づく高精度な三次元位置情報によって、目的移動体４０の正確な位置情報が、高速かつ低コストで得られるようになる。
以上のような本発明の自己位置姿勢標定装置６０を用いた車両等の自動運転制御の詳細については、図１８を参照しつつ後述する。

［自己位置姿勢標定処理］
次に、上述した自己位置姿勢標定装置６０における自己位置姿勢標定処理の詳細について、図１５〜１７を参照しつつ説明する。
ここで、まず自己位置姿勢標定とは、目的移動体（車両、ロボット、航空機、移動する一般的な物体、等）のＣＶ値を決定することである。このＣＶ値を決定したことにより、ＣＶ映像内での移動する物体（目的移動体）の位置と姿勢を一義的に決定することができる。
得られたＣＶ値は、元々は相対座標の相対値であるが、ＣＶ映像地図として、そこに実スケール（絶対座標）を与えることで、ＣＶ値は実スケールを持ち、絶対座標に変換されることになる。

この技術は、自動運転やロボットの走行には欠かせない技術であり、この目的のためには、リアルタイム処理で目的移動体の位置と姿勢（６変数）を求めなければならない。
ただし、ＣＶ映像地図を更新する目的であれば、リアルタイム処理は必要なく、後処理で、取得した新たな映像を自己位置姿勢標定することで、前のＣＶ映像地図の一部分、又は全部を更新することができる。したがって、本発明の自己位置姿勢標定装置はＣＶ映像地図の更新装置としても使用可能となる。

すなわち、本実施形態に係る自己位置姿勢標定が行われることで、ＣＶ値が取得された目的画像は、その後破棄されてしまっても自己位置姿勢標定の目的は達成している。ところが、このようにＣＶ値が取得された目的画像のデータを破棄せず、新しいＣＶ映像地図の一部として利用することもできる。
そのために、ＣＶ値が取得された目的画像をＣＶ映像地図データベースに取り込んで、ＣＶ映像地図に重複するデータとして記憶し、あるいはＣＶ映像地図の一部又は全部と置換して更新することができる。
このように、目的画像と目的画像を取得した６変数を破棄せずに、画像と共に保存することで、自己位置姿勢標定装置としての利用だけではなく、ＣＶ映像地図の更新装置として利用することができる。さらに、ＣＶ映像地図の更新のみを目的として、自動更新のための自動運転車両や自動走行ロボットを運行することも可能である。

以上のように、本発明の前提となるＣＶ演算技術によれば、移動する物体に設置されたカメラから動画像を取得し、特徴点を抽出し、それをフレーム間で追跡し、それを使って、演算によりＣＶ値を求めることができる（図１〜１３参照）。
本発明では、既にＣＶ値が既知であるＣＶ映像地図を三次元地図として用いて、目的移動体の映像又は画像のＣＶ値を、直接演算するのではなく、目的移動体が移動する前に、既に存在しているその地点を含む近隣のＣＶ映像地図を前もって準備し、目的移動体に積載したカメラ映像又はカメラ画像と、既に用意してあるＣＶ映像地図との関係性から、その対応関係を求めることで、目的移動体のＣＶ値を求めるものである。
このようにして得られたＣＶ値を目的ＣＶ値と呼ぶことがある。

そして、本実施形態では、自己位置と姿勢（６変数）を求める装置として、ＣＶ映像地図と目的画像の組み合わせに特徴を有するものである。
特に、自己位置姿勢標定のために必要な画像の特徴点等の種類について、ＣＶ映像地図と目的の移動体との関係性を取るために、以下のような７種の特徴量の少なくともいずれかを用いている。
以下、これを「７種の特徴量」と呼称して説明する。

［７種の特徴量］
以下、図１５，１６を参照しつつ、本実施形態における７種の特徴量について具体的に説明する。
図１５は、図１４に示した自己位置姿勢標定装置６０におけるＣＶ値の移転処理動作の詳細を示すブロック図である。
図１６は、図１５に示すＣＶ値の移転処理動作の具体例を模式的に示す説明図である。
なお、以下に示す特徴量（特徴点）とは、必ずしも面積の無い点ではなく、実際には微少面積を持つ小さな面であったり、特徴の有る形状を持つ面であったり、特徴的属性を持つ領域であることもある。
そのため、本明細書では、特徴点を含めて「特徴量」と呼称して説明する。

［１．特徴点（２Ｄ）］
基準映像となるＣＶ映像内の特徴点は、上述したＣＶ演算処理（図１〜１３参照）で説明したように、画像処理技術により自動的に抽出することができる。このＣＶ映像内の特徴点が、特徴点（２Ｄ）６０ａである（図１５，図１６（ａ）参照）。
この映像内の特徴点は点として定義もできるが、実際には座標的には点と見なせる小領域の画像で有ることが多い。特徴点とは、二次元・三次元に限らず、ＣＶ映像地図内の特徴点をいう場合と、目的の移動体に設置したカメラで取得した画像内の同一地点を示す特徴点をいう場合がある。
特に、特徴点（２Ｄ）は映像内、又は画像内で、二次元量として定義される。
また、特徴点（２Ｄ）は映像の隣接するフレームを跨いで追跡され、ＣＶ演算されることで、三次元特徴点（３Ｄ）になる。

［２．特徴点（３Ｄ）］
上述したＣＶ映像内の特徴点は、ＣＶ映像を生成する過程で、三次元化されるので、三次元の特徴点として扱うことができる。この三次元特徴点が、特徴点（３Ｄ）６０ｂである（図１５，図１６（ｂ）参照）。
三次元特徴点は、図１６（ｂ）に示すように、対応する目的の移動体に設置したカメラで取得した画像（目的画像）内の同一地点を示す特徴点に三次元座標を移転することができる。これにより、対応する目的画像内で取得した画像の一部に三次元座標を与えたことになる。
同様の手法で、画像内の任意の４点以上に三次元座標を移転できれば、画像のカメラ位置と姿勢が求められる。

［３．指定特徴点（２Ｄ）］
上述した特徴点（２Ｄ）６０ａは、画像処理技術により自動生成した特徴点であるが、人間が特徴点をＣＶ映像内に、あるいは目的画像内に指定することで、上記の特徴点（２Ｄ）６０ａと全く同じ扱いをすることができる。
これが、指定特徴点（２Ｄ）６０ｃである（図１５，図１６（ａ）参照）。

［４．指定特徴点（３Ｄ）］
同様に、人間が特徴点を画像内に指定し、その三次元座標をＣＶ映像内に取得して、その三次元座標を求めることで、上述した特徴点（３Ｄ）と同様に扱うことができる。
これが、指定特徴点（３Ｄ）６０ｄである（図１５，図１６（ｂ）参照）。

［５．標識等認識対象物（３Ｄ）］
標識等を上述した二種の指定特徴点６０ｃ，６０ｄとして扱うことができる。
具体的には、認識技術によって取得した標識・看板・地物等の三次元座標付き対象物を、特に標識等認識対象物（３Ｄ）６０ｅとする（図１５参照）。
この標識等認識対象物（３Ｄ）は、既に三次元座標が既知であるから、大きな面積であれば、一つの対象物で目的のＣＶ値を移植することができ、これにより自己位置姿勢標定が可能となる。

［６．実世界三次元マーカ］
上記の標識等のように、初めから実在していた対象物（３Ｄ）ではなく、自己位置姿勢標定を目的として、例えば道路やその周辺などに実世界マーカを設置し、その三次元座標を取得して、あるいは既知として、目的画像内に、その対応点を求めて三次元座標を移植し、そこからＣＶ値を演算で求めることができる。
これが、実世界三次元マーカ６０ｆである（図１５，図１６（ｄ）参照）。
ここで、実世界三次元マーカとは、下記の画像内三次元マーカ（３Ｄマーカ）とは異なるもので、例えば車両の自動運転のためや、ロボットの自動走行のために、自己位置標定を目的として、ＣＶ地図の中だけではなく、積極的に実世界の中に設置される三次元のマーカである。

［７．画像内三次元マーカ］
上記の実世界三次元マーカ６０ｆは、現実世界にマーカを設置する場合であるが、より簡便に、ＣＶ映像地図内に特徴的な場所を選択して、それを画像内三次元マーカ６０ｇとすることができる（図１５，図１６（ｃ）参照）。
この画像内三次元マーカ６０ｇに基づいて、目的画像内に対応箇所を探すことでＣＶ値を取得することができる。

ここで、画像内三次元マーカ（３Ｄマーカ）とは、自動位置姿勢標定装置を効率よく作動させるために、前もってＣＶ映像地図内に、自己位置姿勢の標定を目的として印（マーク）として抽出される、ＣＶ映像地図内に設置された三次元座標を持つ３Ｄマーカである。
具体的には、３Ｄマーカは、元々自動走行のために設置されたものではない対象物を利用することができる。例えば、路面標示やビルの角，窓など、前もって形状や座標を取得しておくことで３Ｄマーカとして利用することができる。
この点において、上述した実世界に実際に設置される三次元マーカと区別することができる。

以上のように、本実施形態では、上記のような７種の特徴量（特徴点）の中の一つ又はいくつかを組み合わせて、演算により目的画像のＣＶ値を求めることができる。
その際に、選択される特徴量として、少なくとも一種が自動抽出され、その選択される特徴量は、既にその位置及びその三次元形状の三次元座標の一部又は全部が、既に自動取得されているものとする。
そのように自動抽出される対象物としては、例えば車両の自動運転の場合には、三次元座標を持つ標識，道路標示，ビルの一部，電柱，縁石等の道路周辺に存在する対象物などである。また、ロボットの自動走行の場合には、活動する範囲内の壁の角，移動しない室内の形状，固定された物体，床面の特徴ある図柄等である。そして、そのような認識対象物は、既に三次元座標が自動演算で求められているものである。

［目的画像への三次元特徴点移転によるＣＶ値取得］
本実施形態における自己位置姿勢標定は、上記のような７種の特徴量（特徴点）の一部又は複数の組み合わせにより、基準となるＣＶ映像から、対象となる目的画像に三次元特徴点を移転することで、目的画像のＣＶ値を取得するものである。
すなわち、基準映像と目的画像の両者の特徴量が座標統合され、目的画像のＣＶ値が取得されることで、自己位置姿勢標定がなされることになる。
これが、図１５に示す対応点演算によるＣＶ移転６１である。

このように、予め用意したＣＶ値が既知であるＣＶ映像地図から、目的画像内の特徴点に三次元座標を移転・移植することで、目的画像のＣＶ値を求めることが、本発明に係るＣＶ値取得である。ただし、目的画像内に既知の三次元点や面があれば、それらを用いることにより、目的画像のＣＶ値取得のための演算コスト等を下げることに貢献することになる。
以下、目的画像のＣＶ値取得のための特徴点の移転・移植の処理動作について具体的に説明する。

［対応点演算によるＣＶ移転］
上述のとおり、対象となる目的画像のＣＶ値（６変数）を求めることが、本発明に係る自己位置姿勢標定の本質である。
特徴点（２Ｄ）を抽出・追跡して、ＣＶ演算によりＣＶ値を得る方法については上述したとおりである（図１〜１３参照）。
そこで、以下では、基準となるＣＶ映像に基づいて目的画像のＣＶ値を取得することで自己位置姿勢標定を行うための方法について説明する。

［１．演算組み込み方式１］
上述したＣＶ演算（図１〜１３）を用いて、目的画像内の特徴点（２Ｄ）を単独でＣＶ演算して、その後に、基準となるＣＶ映像内のＣＶ値と座標統合することができる。ここで座標統合のためには、両者画像内で共通の特徴点を数多く取得する必要がある。
これにより、両者画像内のＣＶ値は同一座標系で表示できるようになり、同一座標系に目的画像内のＣＶ値の取得が完了したことになる。つまり、自己位置姿勢標定が完了したことになる。
ここで、両者画像内とは、基準となるＣＶ映像内と目的画像内との両者画像内を意味する。

［２．演算組み込み方式２］
基準となるＣＶ映像地図のＣＶ映像を、ＣＶ演算以前の状態に戻して、演算に用いた特徴点の三次元座標を用いずに、ＣＶ映像地図と目的画像の両者のフレームと特徴点が混合した状態でＣＶ演算を行う。
これにより、ＣＶ映像地図と目的画像の両者のＣＶ値が求められるが、ここでは両者混合のＣＶ値が求められるときに、ＣＶ映像地図側の特徴点のみの三次元座標を既知として与えることで、目的画像側のＣＶ値が、自動的に精度良く求められることになる。

［３．演算組み込み方式３］
ＣＶ映像地図と目的画像の両者画像内の特徴点（２Ｄ）と特徴点（３Ｄ）、及び三次元座標が既知である点，面などを混在させて、特徴点（３Ｄ）を既知座標として演算に組み込み、これをＣＶ演算することにより、未知であった目的画像内のＣＶ値を取得することで、目的画像のＣＶ値取得が完了する。つまり自己位置姿勢標定が完了する。
すなわち、ＣＶ映像地図・目的画像の両者の二次元特徴点と三次元特徴点が混在する中で、両者を一体としてＣＶ演算を行い、三次元座標が既知の特徴点の三次元座標は固定したまま、全ての特徴点を使ってＣＶ演算を行うことで、目的画像のＣＶ値取得を行うことができる。

［４．３Ｄ特徴点座標移転方式］
基準となるＣＶ映像内の三次元既知点の複数点を、目的画像内の対応点（２Ｄ）に自動対応させることで、三次元座標を目的画像内の対応点に移転する。
そして、三次元座標が既知となった目的画像内の四点以上の複数点の三次元特徴点から、目的画像内のＣＶ値を幾何学的に演算する。
これによって、目的画像のＣＶ値取得が完了し、自己位置姿勢標定が完了する。
ここで、特徴面は、複数の特徴点からなると考えることができ、したがって複数の特徴点の対応と見なすことができる。これはすべて三次元特徴点として扱うため、最も演算コストがかからない方法であり、つまり高速でＣＶ値が求められることになる。

［５．逆３Ｄ特徴点座標移転方式］
この方式は、原理的に基準となるＣＶ映像と目的画像の関係を部分的、限定的に逆転させた場合に相当する。しかしながら、基準画像はあくまでもＣＶ映像地図である。
具体的には、例えば図１６（ｄ）に示すように、実世界に三次元座標の既知の実世界マーカを設置した場合などが相当する。この場合、マーカの三次元座標は基準となるＣＶ映像を介さずに直接的に取得することになる。
ただし、他の特徴点については、ＣＶ映像から取得することになるので、演算内容としては、最終的なＣＶ値取得においては、ＣＶ映像地図が基準画像として係わっていることになる。

なお、上記のような実世界マーカを用いることは、安全等の観点から好ましい。
本発明に係る自己位置姿勢標定装置では、車両等を走行させながらカメラを使用する都合上、どうしても夜間や霧などの場合には、車両等の走行が困難となる場合がある。そのような場合でも、実世界三次元マーカを採用すれば、安全運転の上から、道路上又は道路近傍などに安価かつ安全に設置できることから、実世界３Ｄマーカ方式は安全面等の点で非常に有望である。
もちろん、目的画像内の３Ｄマーカを用いる場合でも、自己位置姿勢標定は十分に可能であるが、行政的配慮などから、法律で定められた実世界３Ｄマーカを用いることが好ましいことになる。

［６．機械センサーによるＣＶ値取得方式］
目的移動体４０（図１４参照）に備えられる機械センサー、例えばＩＭＵ／ＧＹＲＯ／ＧＮＳＳ等により、直接的にＣＶ値を取得することも可能である。
ただし、現在普及している機械センサーは精度が低く、それ単独では実用に耐える精度を得ることは困難である。一方で、機械センサーは、リアルタイム出力が得られることが最大の長所である。
そこで、本実施形態では、比較的低価格のＩＭＵ／ＧＹＲＯ／ＧＮＳＳ等によって、リアルタイムの概略ＣＶ値を得る手法を採用することができる。概略値ではあっても、極短時間であれば、誤差の少ない相対値を得ることができるため、時間不連続のＣＶ値のフレーム間の変動を相対値で知るために、あるいはリアルタイム値を知るために有効となる。
この機械センサーによる補正・補完的なＣＶ値取得の詳細については、後述する図１７を参照しつつ後述する。

［ＣＶ統合演算］
以上のようにして基準となるＣＶ映像に基づいて目的画像のＣＶ値を取得・移植することができるが、ＣＶ映像地図を取得・生成する場合と同じ方法（図１〜１３参照）で、目的画像内のＣＶ値を直接的に取得して、座標を統合して、位置合わせをして、目的画像内のＣＶ値を取得することができる。
これが、図１５に示すＣＶ統合演算６２である。
なお、このような目的画像単独のＣＶ演算・ＣＶ値取得は、単独で用いるものではなく、状況等に応じて、上述した他の方法と併用して一部に用いることになる。

［リアルタイム補正］
原理的に、ＩＭＵ／ＧＹＲＯ等の機械センサーをカメラに取り付ければ、ＣＶ値、つまり６変数を取得することができる。ただし、ＣＶ映像を直接取得するためには、非常に高額（高精度）なＩＭＵ／ＧＹＲＯ等が必要となり、実際には機械式センサー単独での利用は現実的ではない。
一方、ＩＭＵ／ＧＹＲＯ等の機械センサーは、リアルタイム出力が得られるという優れた特徴を持つ。
そこで、本実施形態では、この機械センサーの特徴を有効に活用し、ＣＶ映像、及びＣＶ値取得の演算に伴う時間遅れや時間不連続を補正する、リアルタイム補正として利用するようにしている。

本発明に係るＣＶ演算においては、画像処理が係わる特徴量の演算等は、画像処理時間のために原理的に多少の時間遅れが発生する。
そこで、ＩＭＵ／ＧＹＲＯ等の機械センサーを用いて、微少時間遅れが生じたＣＶの変動をリアルタイムに補正することができる。
具体的には、ＣＶ演算とＣＶ値取得にかかる画像処理時間は、数ミリセカンドから数秒程度である。つまり、この時間のみを機械センサーから得られた６変数で補足することになる。この程度の時間であれば、安価（低精度）なＩＭＵ／ＧＹＲＯ等の機械センサーであっても、時間不連続補正やリアルタイム補正が可能となる。
さらに、ＣＶ値を絶対座標に変換するには、環境内に設置したＧＣＰや、目的画像を取得するカメラに剛体結合されたＧＮＳＳ（ＧＰＳ）によって、取得した相対座標を絶対座標に変換することができる。

［機械センサーによる目的ＣＶ値の高精度化］
以下、図１７を参照しつつ、機械センサーを用いたＣＶ値の高精度化（補正・補完）について具体的に説明する。
図１７は、本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置により得られる目的画像のＣＶ値を機械センサーで得られる６変数により高精度化する場合の処理を模式的に示した説明図であり、（ａ）は目的画像を構成する複数フレームの全体を、（ｂ）は（ａ）に示す複数フレームの一部を拡大して示したものである。

ここで、図１７においては、目的画像のＣＶ値を縦軸にとるが、６変数のすべてを表示すると煩雑となり理解の妨げとなるため、目的ＣＶ値の変数の１つのみを縦軸［０１］にとる。また、横軸［０８］は時間経過であり、時間軸の一区切りは、目的カメラのフレーム間隔に対応している。
ＣＶ映像地図と目的画像の比較で取得したＣＶ値［０２，０６，１０］は、●で示している。
機械センサーで取得した６変数の内の１つを、点線１［０５］で示す。
図１７（ｂ）に示すように、機械センサーはリアルタイム出力であるから、目的画像から取得した目的ＣＶ値は時間遅れΔｔ［０４］が発生している。
さらに、機械センサーの精度不足から、変数値そのものに、誤差Δｄ［０３］が発生している。

このような出力が得られた場合に、目的画像から取得したＣＶ値の遅延時間を既知として、時間軸をずらして、リアルタイム軸に戻して機械センサーからの出力と重ね合わせる。
そして、目的画像から得られたＣＶ値を真値として、機械センサーの同時刻のＣＶ値と、フレーム両端の値を合わせるように補正する。この場合、平行移動だけで合致しないときは、比例配分して合致させる。
このようにすることで、フレーム間は機械センサーからのＣＶ値で補完することができたことになる。このようにして、現在時刻でのリアルタイムＣＶ値を取得することに、自己位置姿勢標定装置としての意味がある。

ところで、精度の高いリアルタイムのＣＶ値を記録保存するためには、上記のようになるが、現在時刻のＣＶ値のみを必要とする自動運転のための自己位置姿勢標定の場合には、１フレームを超えて補正する必要が出て来る場合があり、次のようになる。すなわち、目的画像から取得したＣＶ値は、演算処理時間のために、常にΔｔの遅延がある。このとき、Δｔが１フレーム期間を超えることがある。その期間は、機械センサーで補うことになる。このとき、フレーム両端ではなく、最終のフレーム［１０］から、現在時刻［１２］までの機械センサーのデータ［０５］で、最終ＣＶ値［１０］から延長させて、現在時刻の目的ＣＶ値［１１］を得ることになる。
なお、図１７では、演算遅延時間が１フレーム内として図示したが、１フレームを超えても、意味は変わらない。Δｔ［０４］の長さが、さらに図面左に伸びることになるだけである。
以上によって、最終的に目的カメラのＣＶ値はリアルタイム性が確保されたことになる。

以上のように、本実施形態では、要求精度を満たさない比較的低精度で安価なＩＭＵ／ＧＹＲＯ等の機械センサーにより、目的の移動体（目的移動体）の自己位置姿勢を標定する場合、目的の移動体（目的移動体）の自己位置と姿勢（６変数）を当該機械センサーにより、一旦誤差を含んだまま求める。
そして、その累積誤差を補正するために、ＣＶ映像地図と、当該目的移動体に積載したカメラ画像を比較して得た、より精度の高いＣＶ値でもって機械センサーで得た６変数を間欠的に補正し、時間的に遅延のない、連続な自己位置姿勢の６変数を取得することができるものである。

このように、機械センサーから取得した生のデータの６変数は、時間的に連続であり、リアルタイム性がある反面、ＣＶ映像地図のＣＶ値と比較して、機械センサーの方がかなり低精度である。
一方、ＣＶ映像地図によるＣＶ値は、演算による時間遅れと、時間的に断片値であり、連続性に欠けるなど、一長一短がある。
そこで、両者を組み合わせることで、両者の長所を引き出せるようになる。

このようにＣＶ映像地図と機械センサーを組み合わせることにより、演算処理による遅延誤差を補正する効果が期待できるだけではなく、リアルタイム性を同時に持つことになる。
すなわち、目的画像のＣＶ値（６変数）取得の演算処理時間が有限であるために遅延が生じる。これは自己位置標定と姿勢の誤差となる。その遅延時間内に生じるＣＶ値の進行を補正するために、目的画像のＣＶ値の空白となる直近の終端から現在時刻までの極小時間の期間について、機械センサーで内挿することで、結果としてリアルタイム性を向上させることができる。
そもそも、機械センサーのみで精度を出すためには高額な設備・装置等が必要となり、それ以外にも、キャリブレーションが困難になるなどの問題があった。

そこで、本発明に係るＣＶ演算技術を用いて、精度の高い目的カメラのＣＶ値を求めることで、機械センサーとＣＶ映像地図による補正・補完が可能となる。
すなわち、精度は低いが遅延の無いＩＭＵ／ＧＹＲＯ等の機械センサーによるリアルタイム出力の位置姿勢の６変数を内挿することができる。機械センサーのリアルタイム出力は、精度が低いが、極超短時間であれば、誤差の少ない６変数を取得できるという特性を利用している。
このようにして、ＣＶ値は基本的にＣＶ演算で求めるが、直近の極超短時間のみ機械センサーで補完して、リアルタイム補正するものである。

［自動運転システム］
次に、以上のような本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置による目的移動体の自動運転について、図１８を参照しつつ説明する。
図１８は、本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置による移動体の自動運転システム１００のシステム構成を示す機能ブロック図である。
なお、図１８に示す自動運転システム１００のシステム構成では、上述した自己位置姿勢標定装置（図１４参照）に直接関係しない部分を点線で、自己位置姿勢標定装置に直接関連する部分を実線で示している。

自動運転システム１００では、目的移動体（この場合は自動運転車両）に設置された目的画像を取得する目的画像取得／目的カメラ部１０１と、前もって作成されたＣＶ映像地図部１０２と、両者を比較する特徴点比較部１０５で、両者画像の対応点を取る。これによって、まず、演算遅延を持つ目的ＣＶ値が取得される。
次に、機械センサー／６変数取得部１０６から取得した低精度ではあっても、リアルタイム出力のＣＶ値が出力される。
次いで、自己位置姿勢標定部１０９で、特徴点比較部１０５から出力されたＣＶ値が、機械センサー／６変数取得部１０６から出力される遅延の無いＣＶ値で補正され、遅延の無い最終の目的ＣＶ値が生成・出力される。

以上のような自己位置標定装置は、自動運転の主たる装置であるが、自動運転を行うには、その他の構成として、３Ｄ空間／環境属性識別部１０３で、屋外，屋内，交差点，道路上，道路外，トンネル等の環境の大まかな属性が把握される。
また、対象物認識部１０４において、走行路付近の対象部が認識され、その三次元座標が取得される。
そして、障害物３Ｄ認識部１０７により、走行路付近の対象物が障害物で有ると判断されれば、運転パラメータ指示部１１１に信号が送られる。

このようにして、当該自動運転車両の周辺の、並進車両，対向車両，駐車車両，人，その他の移動体などが認識され、並進車両，対向車両については、大きさとその６変数が、駐車車両については、大きさとその三次元座標が、人やその他の移動体については、大きさと移動方向等の情報が取得され、運転パラメータ指示部１１１に信号が送られる。
運転パラメータ指示部１１１は、運転条件設定部１１０のコントロール下にあり、運転に必要な条件が設定され、最終的に車両の案内自動走行部１１２によって、直接車両がコントロールされることで、自動運転が実行される。

以上のように、本実施形態の自動運転システム１００は、上述した基準映像地図を用いた自己位置標定装置１を利用して、正確かつ安全な移動車両等の自動運転・自動走行が可能となる。
なお、上記のような自己位置姿勢標定装置が何らかの理由で故障した場合などには、車両を安全に誘導し、安全に停止させなければならない。高速道路の場合などには、すぐに止まれない場合も想定される。
そこで、通常は自己位置姿勢標定装置を作動させた自動運転としながらも、緊急時には、外からの信号を一切受けずに、自ら取得した情報のみで、車両を安全に誘導し、停止させる構成を備えることが必要となる。

自動運転装置・システムには、必ず障害物検出装置が設置されるから、障害物検出装置が設置された装置側を補助装置として、常に本装置と補助装置の二装置を作動させながら、自動運転を行うことが望ましい。
また、ロボットにおいても同様に、自己位置姿勢装置が故障しても、自動で安全な場所に移動して、停止させる補助装置が必要である。
このように、本実施形態では、自己位置姿勢標定装置以外に、目的車両に積載した、本装置とは独立した自律走行システムを積載して、緊急時には、他のカメラ、又は他のセンサーにより、目的車両を安全に誘導し、停止させることができる安全装置付きの自己位置姿勢標定装置を提供することができる。

［実施例］
次に、本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置の、より具体的な実施例について説明する。
以下では、自動運転における標準的な自己位置姿勢標定装置の実施例を示す。
なお、以下の実施例では、適宜上述した図１及び図６を参照しつつ説明する。

前もってＣＶ映像取得装置１０で得られた環境を撮影した映像からＣＶ演算して、ＣＶ映像地図作成装置２０により、ＣＶ映像地図データベース３０を作成する。
次に、ＣＶ映像データベース３０から、自動運転に必要な範囲の地図のみ切り出して、ＣＶ映像地図・目的画像比較装置５０で、目的カメラのＣＶ値を取得する。
一方、同時に目的移動体４０にカメラと一体化された装置で、まず機械センサーにより、直接６変数を取得する。これで、同時にカメラからの画像で取得したＣＶ値と、機械センサーで取得したＣＶ値、即ち両者の６変数を取得したことになる。

この二種のデータの内、機械センサーにより直接得られた６変数は、時間的に連続的だが（図１７［０５］参照）、精度が低い。一方、画像は断片的データ［図１７［０６］参照］だが、精度が高い。
そこで、自己位置姿勢標定装置６０において、連続した機械センサーのＣＶ値の中で、目的カメラのＣＶ値のデータと一致する時間（フレームの整数倍）でキャリブレーションして機械センサーのＣＶ値を補正し（図１７［０４／０３］）、その中間は機械センサーのデータで内挿して、全体として、精度が向上したＣＶ値を取得する。

自己位置姿勢標定装置６０で取得された信号は、車両周囲状況判断装置７０に送られ、ＣＶ映像地図内に位置決めされ、同時に周囲の対象物や歩行者，障害物等とともに位置決めされる。
これらの対象物等が三次元的に、そしてそれぞれの位置関係が明らかになり、車両制御信号発生装置８０で、車両の制御信号が生成され、車両（目的移動体４０）が自動制御されて、自動運転が実現される。

なお、目的画像のＣＶ値を求めることが、自己位置姿勢標定そのものであるが、必ずしも目的画像の全フレームを特徴点追跡によって求めなくてもよい。
一般に、演算にはそれなりの時間をかけなければ精度の高い値（ＣＶ値）は求められないので、できるだけ演算時間をかけることが望ましいが、その分フレームは落ちてくることになる。
そこで、本実施例では、目的画像側では飛び飛びのフレームでＣＶ取得して、その飛ばされたフレームに関しては演算せずに、同時に取得した機械センサーから得た６変数により埋めるようにする。

具体的には、以下のようにして行うことができる。
［実施例１］
目的画像が６ｆｐｓ（毎秒６フレーム）である場合、ＣＶ演算には一秒かけて、つまり１／６のフレームのみＣＶ演算する。
これにより、１フレームのみ高精度演算を行い、残りの５フレームについては、機械センサーから取得した６変数を内挿することができる。

［実施例２］
図１７に示したような、目的画像が０．５ｆｐｓ（２秒に１フレーム）である場合に、ＣＶ演算に０．３秒がかかり、したがって、０．３秒のディレー（遅延）が発生するとする。
この場合、機械センサーで、０．３秒間を補完して、リアルタイムＣＶ値を出力することができる（図１７参照）。

勿論、この場合、機械センサーによる誤差が、ＣＶ演算の誤差より少ないか、同等であることが条件となる。この条件は極めて妥当な条件である。
もし、目的ＣＶ演算に一秒かかれば、これは機械センサーから得られる６変数を一秒に一回、キャリブレーションしていることになる。
機械センサーの１秒間で発生する累積誤差が、ＣＶ演算による誤差を下回っているという条件は、安価なＩＭＵやＧＹＲＯでも、十分に対応できることを意味する。したがって、これは極めて現実的な方法となる。
なお、目的画像から得られた目的ＣＶ値も、機械センサーも、それぞれ誤差を持つので、補正期間とカメラフレーム数、機械センサーの性能等の兼ね合いで、カメラフレーム数を決定することになる。

以上説明したように、本実施形態の基準映像地図を用いた自己位置姿勢標定装置１によれば、基準映像となるＣＶ映像地図として、移動するカメラによる映像又は連続する静止画像（２次元座標）と、当該カメラの位置と姿勢を示す３次元座標（Ｘ，Ｙ，Ｚ）及び回転量（Φｘ，Φｙ，Φｚ）の計６変数をＣＶ値として全フレームに付与し、それによっていつでも、映像内の任意の場所の３次元座標を取得することができる状態とすることができる。
したがって、自動運転等を行う環境の全域の三次元点（点群）を保持しなくても、必要なときに数ミリセカンドの演算処理を行うことで、いつでも任意の点の三次元点の位置情報を取得・生成することができ、三次元空間を示すデータを大幅に圧縮できるようになる。
これによって、車両や航空機などの自動運転、三次元空間を移動するロボットの自動走行等において、移動体自身の自己位置と姿勢を示す情報を、簡易・迅速かつ高精度に知ることができるようになる。

上述したように、従来の自己位置姿勢標定に用いられているＳＬＡＭやＶ−ＳＬＡＭは、最初から全域の三次元点を持つために、しかも膨大な点群データをやりとりするために、データの転送や記録に膨大な演算コストや費用が発生し、広い地域を自動運転し、自己位置姿勢の標定を行うには、現実的ではなかった。
ＳＬＡＭやＶ−ＳＬＡＭでは通常、自動運転に必要な点群は、概略数億点／ｋｍにもなる。
本実施形態の自己位置姿勢標定装置においても、実際にはいくつかの特徴ある三次元点のデータを保持することになるが、それが仮に数百点／ｋｍの三次元点を持ったとしても、従来のＬＩＤＡＲ方式やＶ−ＳＬＡＭによる点群の数とは比較にならないほど微少であり、わずか数百点のために増加するデータ量は無視できる量である。

実際にデータとして持つのは、空間の三次元座標ではなく、カメラ位置の６変数のみである。そして、それでありながら、この６変数により、画像内のすべての点の三次元座標を簡単な演算で求めることができる状態で３Ｄ地図として利用できることが、本発明の優れた特徴である。
ＣＶ映像地図は、前もって自動走行する目的の環境内を移動する基準カメラによる基準映像又は連続する基準静止画像（２次元画像）と、当該環境の基準ＣＶ値を生成しておくことで、つまり、二次元のままの映像と、その各フレームに６変数を対応させることで、当該環境の三次元情報を集約して保持できるものである。
しかも、ＣＶ映像地図は、膨大な三次元点群を持つことなく、当該ＣＶ映像地図内の任意の点の三次元座標を、必要な時に、演算で求めることができる状態にしておくことで、データを大きく軽量化して保持・管理できることが特徴である。

このように、本発明で基準映像として用いるＣＶ映像地図は、一手間をかけることで、いつでもどこでも、環境内の三次元座標を取得できる状態で保持されていることから、データは極めて軽量であり、通信にも十分耐えうることになり、自動運転を現実的なものとすることができる。
そして、このようなＣＶ映像地図を基準画像として、これと目的画像を比較し、両者の同一箇所を示す複数の特徴点を自動的に対応させ、目的画像のＣＶ値（６変数）を演算で取得することで、目的画像自己位置姿勢を、迅速に取得することができるものである。

したがって、本実施形態に係る基準映像地図を用いた自己位置姿勢標定装置１では、ＣＶ映像と目的画像との比較、及び機械センサーとの組み合わせによって、以下のような優れた効果を実現することができる。
すなわち、ＣＶ映像地図と目的画像を組み合わせることで、第一に、取り扱うデータが軽くなり、演算が効率的になる。
第二に、カメラから取得した動画像と、カメラから取得した目的画像のＣＶ値に、機械センサーでの補正を加えることができる。これらの機器構成としては単純であり、堅牢であり、扱いやすく、低価格で、高性能な自己位置姿勢標定装置が得られる。
第三に、機械センサーとしてのＧＮＳＳからの出力を一つの特徴点として、あるいは直接既知のＣＶ値として、ＣＶ演算に組み入れることで、簡単に目的ＣＶ値の高精度化が可能となる。

さらに、ＩＭＵ・ＧＹＲＯによる時間不連続補正を行うことができる。
このようなＣＶ映像の精度向上は、後処理で、撮影後でも可能であり、精度に合わせた後処理や設計等が可能であり、さらに高精度を要求されれば、いつでも高精度化が可能である。
これに対して、上述した特許文献１に開示されているような従来の自己位置姿勢標定方法では、レーザー点群による３Ｄ地図は後の精度向上が極めて困難で、一度取得したデータを変更等することは極めて困難である。

以上、本発明の基準映像地図を用いた自己位置姿勢標定装置について、好ましい実施形態を示して説明したが、本発明に係る基準映像地図を用いた自己位置姿勢標定装置は、上述した実施形態にのみ限定されるものではなく、本発明の範囲で種々の変更実施が可能であることは言うまでもない。
例えば、上述した実施形態においては、本発明の基準映像地図を用いた自己位置姿勢標定装置の適用対象として、車両等の移動体の自動運転を想定して説明したが、本発明に係る自己位置姿勢標定装置は、自己位置姿勢標定が必要となるどのような装置や手段にも応用できるものであり、その用途・使用方法等も特に限定されるものでないことは言うまでもない。

また、上述した実施形態では、自動運転等を行う場合に、基準となる三次元地図として、前もってＣＶ値を高精度で求めておいた基準映像としてのＣＶ映像（全フレームにＣＶ値を持った動画映像）を用いて、車両積載のカメラから取り込んだリアルタイム映像を目的画像として、本発明によりＣＶ値の移植・統合を行うことで、自車両の位置を高精度に取得できることを説明した。
ただし、基準映像を、機械センサーや測量装置等を用いて実測により生成した三次元地図を基準としても同様となる。
したがって、本発明に係る自己位置姿勢標定装置は、機械センサー等の実測値のデータを三次元座標データとして併用することが可能である。

また、本発明によれば、位置精度としては、リアルタイムＧＰＳの百倍以上の位置精度を持つことが期待できることから、ＧＰＳを本発明における概略位置設定手段として利用することもできる。
また、上述したように、三次元地図を更新する場合にも、本発明の基準映像地図を用いた自己位置姿勢標定装置は効果的に用いられ、映像から地図を生成する際の更新にも当然利用することができる。
さらに、本発明によれば、ＧＰＳを用いないでも高精度の位置座標を取得できることから、高精度ナビゲーション技術への利用も期待できる。

本発明は、例えば自動車などの各種車両や航空機，船舶等の移動体の自動運転や、ロボット等の自動走行などに好適に利用することができる。

１０ ＣＶ映像取得装置
２０ ＣＶ映像地図作成装置
３０ ＣＶ映像地図データベース
４０ 目的移動体
５０ ＣＶ映像地図・目的画像比較装置
６０ 自己位置姿勢標定装置

Claims (10)

1. 所定の映像取得手段で撮影された基準映像に基づいて、当該基準映像のカメラ位置と姿勢の三次元座標値を示すＣＶ（カメラベクトル）値を求めるＣＶ演算を行い、前記基準映像に前記ＣＶ値を付加したＣＶ映像地図を生成するＣＶ映像地図作成手段と、
   前記ＣＶ映像地図を記憶するＣＶ映像地図データベースと、
   前記ＣＶ映像地図データベースに記憶されたＣＶ映像地図を基準画像とし、目的移動体に備えられた所定の画像取得手段で撮影された目的画像を前記ＣＶ映像地図と比較して、当該目的画像とＣＶ映像地図の同一箇所を示す複数の特徴点を自動的に対応させることにより、当該目的画像のＣＶ値を取得する自己位置姿勢標定手段と、
   を備え、
   前記自己位置姿勢標定手段が、
   前記ＣＶ映像地図と前記目的画像を組み合わせて、前記ＣＶ映像地図及び前記目的画像に含まれる二次元特徴点及び三次元特徴点を一体として前記ＣＶ演算を行い、三次元座標が既知の前記ＣＶ映像地図の特徴点の三次元座標は変数としてのＣＶ演算を行うことなく固定して、前記ＣＶ映像地図及び前記目的画像に含まれる全ての特徴点についてのＣＶ演算を行うことで、前記目的画像のＣＶ値を取得するとともに、
   前記ＣＶ映像地図及び前記目的画像に含まれる全てのＣＶ値から、前記目的画像のＣＶ値を分離して取り出し、自己位置標定のＣＶ値とする
   ことを特徴とする基準映像地図を用いた自己位置姿勢標定装置。
2. 前記ＣＶ映像地図作成手段は、
   前記基準映像に写し込まれた三次元空間の三次元情報を、そのまま空間の三次元座標データとして保持することなく、当該基準映像を取得した時の前記ＣＶ（カメラベクトル）値である、当該基準映像を撮影した前記映像取得手段の位置と姿勢を示す６変数のデータとして生成し、
   前記ＣＶ映像地図データベースは、
   前記ＣＶ値から、前記基準映像中の任意の点の三次元座標を随時求めることが可能となる、三次元データが軽量化された、基準映像とＣＶ値とが対応して保持された前記ＣＶ映像地図を記憶する
   ことを特徴とする請求項１記載の基準映像地図を用いた自己位置姿勢標定装置。
3. 前記自己位置姿勢標定手段が、
   前記目的移動体に備えられた機械センサーで取得される、当該目的移動体の自己位置と姿勢を示す６変数データを遅延のないデータとして扱い、前記目的画像のＣＶ値の演算遅延時間を換算して、リアルタイム値に補正することにより、リアルタイムＣＶ値、若しくは、時間的に連続した前記目的移動体の自己位置と姿勢を示す６変数を取得する
   ことを特徴とする請求項１又は２記載の基準映像地図を用いた自己位置姿勢標定装置。
4. 前記自己位置姿勢標定手段が、
   前記目的画像とＣＶ映像地図の同一箇所を示す特徴点として、前記ＣＶ映像地図に含まれる所定の特徴量を選択し、
   前記所定の特徴量として、前記ＣＶ映像地図に含まれる、自動抽出された三次元座標を持つ標識等の三次元認識対象物を選択する
   ことを特徴とする請求項１〜３のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
5. 前記自己位置姿勢標定手段が、
   前記目的画像とＣＶ映像地図の同一箇所を示す特徴点として、前記ＣＶ映像地図に含まれる所定の特徴量を選択し、
   前記所定の特徴量として、前記ＣＶ映像地図に含まれる、三次元座標が既知である画像内三次元マーカを選択する
   ことを特徴とする請求項１〜４のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
6. 前記自己位置姿勢標定手段が、
   前記目的画像とＣＶ映像地図の同一箇所を示す特徴点として、前記ＣＶ映像地図に含まれる所定の特徴量を選択し、
   前記所定の特徴量として、前記ＣＶ映像地図に含まれる、三次元座標が既知である実世界三次元マーカを選択する
   ことを特徴とする請求項１〜５のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
7. 前記自己位置姿勢標定手段が、
   前記ＣＶ値が取得された目的画像を前記ＣＶ映像地図データベースに取り込んで、当該ＣＶ映像地図データベースに記憶されたＣＶ映像地図の一部又は全部を更新する
   ことを特徴とする請求項１〜６のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
8. 前記自己位置姿勢標定手段が、
   前記ＣＶ映像地図と前記目的画像を組み合わせて、前記ＣＶ映像地図から前記目的画像へ、又は、前記目的画像から前記ＣＶ映像地図へ、三次元特徴点の三次元座標を移転することにより、前記目的画像のＣＶ値を取得する
   ことを特徴とする請求項１〜７のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
9. 前記目的移動体が、自律走行システム、及びカメラ又はセンサーを備えた目的車両からなり、
   当該目的車両が、自己位置姿勢標定手段で生成される前記ＣＶが付加された前記目的画像に基づいて誘導及び停止される
   ことを特徴とする請求項１〜８のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。
10. 前記ＣＶ映像地図作成手段が、
    動画映像の画像データから、所定数の特徴点を自動抽出する特徴点抽出部と、
    抽出された特徴点について、動画映像の各フレーム画像内で自動追跡してフレーム画像間での対応関係を求める特徴点対応処理部と、
    対応関係が求められた特徴点の三次元位置座標を求め、当該三次元位置座標から、各フレーム画像に対応したカメラの三次元位置座標及び三次元回転座標からなるカメラベクトルを求めるカメラベクトル演算部と、を備える
    ことを特徴とする請求項１〜９のいずれか一項記載の基準映像地図を用いた自己位置姿勢標定装置。