JP6479296B2 - 位置姿勢推定装置および位置姿勢推定方法 - Google Patents

Description

この発明は、推定対象物の位置および姿勢を推定する位置姿勢推定装置および位置姿勢推定方法に関する。

撮影画像から画像認識された距離標の位置情報を用いて、推定対象物の位置および姿勢を推定する技術が知られている。例えば、特許文献１には、キロポスト（距離標）の位置情報を用いて車両位置を補正する車両位置検出装置が記載されている。この車両位置検出装置は、予め記憶しておいたキロポストの位置情報と特徴情報のうち、撮影画像から画像認識した特徴情報に対応するキロポストの位置情報を特定し、この位置情報を用いて車両位置を補正している。

特開２００９−２５０７１８号公報

しかしながら、特許文献１に記載される車両位置検出装置では、同じ特徴情報を有する複数のキロポストがそれぞれ異なる場所に存在した場合、撮影画像から画像認識した特徴情報のみでは、カメラで撮影されたキロポストを正確に特定できない。このため、誤った位置に補正される可能性があった。

例えば、上り線と下り線とに車線が分離された道路において、下り線の始点と上り線の終点とを起点として１００メートルごとにキロポストが設置されている場合を考える。
キロポストには、道路に設定された起点から当該キロポストまでの距離であるキロ数が標示されるので、上り線側のキロポストとこれに対応する位置の下り線側のキロポストとには同じキロ数が標示される。上記車両位置検出装置では、このキロ数が特徴情報となるため、上り線側のキロポストの位置情報を特定すべきところ、これに対応する位置にある下り線側のキロポストの位置情報が誤って特定される可能性がある。

この発明は上記課題を解決するもので、推定対象物の位置および姿勢の推定精度を高めることができる位置姿勢推定装置および位置姿勢推定方法を得ることを目的とする。

この発明に係る位置姿勢推定装置は、対象物検索部、方位算出部、対象物特定部および位置姿勢推定部を備える。
対象物検索部は、認識対象物に設定された基準点の３次元空間における位置座標と当該認識対象物が標示する標示情報とが対応付けられたデータベースから、撮影画像から認識された標示情報に対応する認識対象物を検索する。
方位算出部は、対象物検索部によって検索された認識対象物に設定された基準点の位置座標に基づいて、認識対象物の方位を算出する。
対象物特定部は、撮影画像を撮影した推定対象物の撮影方位と方位算出部によって算出された認識対象物の方位とに基づいて、対象物検索部によって検索された認識対象物の中から、撮影画像における認識対象物を特定する。
位置姿勢推定部は、対象物特定部によって特定された認識対象物に設定された基準点の３次元空間における位置座標と撮影画像における位置座標との対応関係に基づいて、推定対象物の位置および姿勢を推定する。

この発明によれば、推定対象物の撮影方位と認識対象物の方位とに基づいて、同じ標示情報を標示する認識対象物の中から撮影画像における認識対象物を正確に特定することができる。正確に特定された認識対象物の基準点の３次元空間における位置座標と上記撮影画像における位置座標との対応関係に基づいて、推定対象物の位置および姿勢を精度よく推定することができる。

この発明の実施の形態１に係る位置姿勢推定装置の機能構成を示すブロック図である。 距離標データベースの例を示す図である。 図３Ａは、実施の形態１に係る位置姿勢推定装置の機能を実現するハードウェア構成を示すブロック図である。図３Ｂは、実施の形態１に係る位置姿勢推定装置の機能を実現するソフトウェアを実行するハードウェア構成を示すブロック図である。 実施の形態１に係る位置姿勢推定装置の動作を示すフローチャートである。 撮影画像から距離標を認識する処理の概要を示す図である。 距離標の方位を算出する処理の概要を示す図である。 距離標の特定処理の概要を示す図である。 距離標の設置例を示す図である。

以下、この発明をより詳細に説明するため、この発明を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
図１は、この発明の実施の形態１に係る位置姿勢推定装置１の機能構成を示すブロック図である。位置姿勢推定装置１は、図１に示すように、撮影装置２、センサ装置３および距離標データベース４のそれぞれと接続し、撮影装置２の位置および姿勢を推定する。
撮影装置２は、位置姿勢推定装置１によって位置および姿勢が推定される推定対象物であり、車両の予め定められた箇所に設置されて当該車両の周辺を撮影する。
以下の説明において、“位置”は３次元空間における推定対象物の位置を示しており、“姿勢”は３次元空間における推定対象物の傾き角または回転角などである。撮影装置２は、上記車両の前方を撮影する車載カメラであるものとする。

センサ装置３は、上記車両に搭載されたセンサであり、３軸の加速度センサおよび３軸の地磁気センサである。上記加速度センサは、３次元空間におけるｘ軸に沿った加速度、ｙ軸に沿った加速度、ｚ軸に沿った加速度を検出する。上記地磁気センサは、ｘ軸、ｙ軸およびｚ軸のそれぞれに沿った地磁気を検出する。
なお、上記車両において、撮影装置２とセンサ装置３との位置関係は固定されている。

距離標データベース４は、距離標に設定された基準点の３次元空間における位置座標と当該距離標のキロ数とが対応付けられたデータベースである。
距離標は、撮影装置２によって撮影された撮影画像から、キロ数が画像認識される認識対象物である。キロ数は、認識対象物である上記距離標に標示された標示情報であって、上記道路に設定された起点から上記距離標までの距離を示している。

図２は、距離標データベース４の例を示す図である。図２において、距離標は、矩形の標識板であり、基準点は、当該距離標においてキロ数が標示された面の四隅の点である。図２に示す距離標データベース４に登録された距離標は、上り線と下り線とに車線が分離された道路に設定された距離標である。また、距離標データベース４において、同じキロ数を標示する距離標は、上り線側の距離標とこれに対応する位置にある下り線側の距離標である。

距離標データベース４に登録された基準点は、現実空間を表す３次元空間における点である。上記３次元空間は、例えば、地球上にある特定の点を原点として、東西方向にｘ軸（例えば、東方向が正）を規定し、南北方向にｙ軸（例えば、北方向が正）を規定し、標高方向にｚ軸（例えば、距離標の高さ方向を正）を規定した３次元座標系で表される。
上記四隅の点の位置座標は、上記原点から上記四隅の点までのｘ軸、ｙ軸およびｚ軸のそれぞれに沿った距離をメートル単位で表したものである。
以下、上記現実空間を表す３次元座標系を“世界座標系”と呼ぶ。

位置姿勢推定装置１は、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０を備える。
距離標認識部５は、対象物認識部に相当する構成要素であり、撮影装置２によって撮影された撮影画像から、距離標に標示されたキロ数と当該距離標に設定された基準点の上記撮影画像の２次元座標系における位置座標とを認識する。
例えば、距離標認識部５は、２次元実写画像である撮影画像にパターン認識処理または文字認識処理を行うことで、当該撮影画像に写った距離標のキロ数と基準点の位置座標とを認識する。
以下、撮影画像の２次元座標系を“画像座標系”と呼ぶ。

方位推定部６は、センサ装置３の検出情報に基づいて、撮影装置２の撮影方位を推定する。撮影装置２の撮影方位とは、撮影装置２の視点がある方位である。この方位は、撮影装置２の撮影方位を示す角度であればよく、例えば、北を０度、東を９０度、南を１８０度、西を２７０度で表してもよいし、角度をラジアン単位で表してもよい。

距離標検索部７は、対象物検索部に相当する構成要素であり、距離標認識部５によって撮影画像から認識されたキロ数に対応する距離標を距離標データベース４から検索する。図２に示した距離標データベース４には、上り線側の距離標とこれに対応する位置の下り線側の距離標とが同じキロ数であるので、距離標認識部５によって認識されたキロ数に対応する２つの距離標が検索される。例えば、距離標認識部５によって認識されたキロ数が１２３．４であると、このキロ数に対応する２つの距離標が検索され、これらのそれぞれに設定された基準点の位置座標が方位算出部８に出力される。

方位算出部８は、距離標検索部７によって検索された距離標に設定された基準点の位置座標に基づいて、キロ数が標示された面の法線方向を距離標の方位として算出する。
距離標の方位は、撮影装置２に対する距離標の方位を示す角度であればよく、例えば、方位推定部６と同様に、北を０度、東を９０度、南を１８０度、西を２７０度で表してもよいし、角度をラジアン単位で表してもよい。

距離標特定部９は、対象物特定部に相当する構成要素であり、撮影装置２の撮影方位と距離標の方位とに基づいて、距離標検索部７によって検索された距離標の中から、撮影画像における距離標を特定する。ここで、撮影装置２の撮影方位は、方位推定部６によって推定された方位であり、距離標の方位は、方位算出部８によって算出された方位である。

位置姿勢推定部１０は、距離標特定部９によって特定された距離標に設定された基準点の３次元空間における位置座標と撮影画像における位置座標との対応関係に基づいて、撮影装置２の位置および姿勢を推定する。
例えば、位置姿勢推定部１０は、基準点の世界座標系における位置座標と画像座標系における位置座標との対応関係に基づいて、撮影装置２の内部パラメータを用いたＰｎＰ（Ｐｅｒｓｐｅｃｔｉｖｅ ｎ−Ｐｏｉｎｔｓ）問題を解く。これにより、撮影装置２の位置および姿勢が推定される。上記内部パラメータ（ｉｎｔｒｉｎｓｉｃ ｐａｒａｍｅｔｅｒｓ）は、撮影装置２が備える撮影レンズの焦点距離と主点とを含む情報である。

距離標データベース４は、位置姿勢推定装置１とは別に設けられた外部記憶装置の記憶領域上に構築されていてもよい。この場合、距離標検索部７は、インターネットまたはイントラネットなどの通信回線を経由して上記外部記憶装置における距離標データベース４にアクセスして距離標を検索する。
距離標認識部５は、撮影装置２が備える構成要素であってもよい。この場合、距離標認識部５は、位置姿勢推定装置１から除かれて、撮影画像から認識された標示情報は、撮影装置２から距離標検索部７に出力され、撮影画像から認識された基準点の位置座標は、撮影装置２から位置姿勢推定部１０に出力される。
方位推定部６は、センサ装置３が備える構成要素であってもよい。この場合、方位推定部６は、位置姿勢推定装置１から除かれて、撮影装置２の撮影方位を示す情報は、センサ装置３から距離標特定部９に出力される。

図３Ａは、位置姿勢推定装置１の機能を実現するハードウェア構成を示すブロック図である。撮影装置１００、センサ装置１０１、記憶装置１０２、および処理回路１０３は、バスによって互いに接続されている。図３Ｂは、位置姿勢推定装置１の機能を実現するソフトウェアを実行するハードウェア構成を示すブロック図である。撮影装置１００、センサ装置１０１、記憶装置１０２、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０４、およびメモリ１０５は、バスによって互いに接続されている。

図３Ａおよび図３Ｂにおいて、撮影装置１００は、位置姿勢推定装置１を搭載する車両周辺を撮影する撮影装置２であり、センサ装置１０１は、センサ装置３を構成するセンサである。記憶装置１０２は、距離標データベース４を記憶している。記憶装置１０２は、例えば、ＲＡＭ （Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ （Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＨＤＤ （Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などで実現され、これらを組み合わせた記憶装置であってもよい。また、記憶装置１０２の記憶領域の一部または全部を外部装置に設けてもよい。この場合、位置姿勢推定装置１が、インターネットまたはイントラネットなどの通信回線を経由して上記外部装置と通信して距離標の検索処理が実行される。

位置姿勢推定装置１における、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０の各機能は、処理回路により実現される。すなわち、位置姿勢推定装置１は、これらの機能を実行するための処理回路を備える。処理回路は、専用のハードウェアであってもメモリに格納されるプログラムを実行するＣＰＵであってもよい。

処理回路が図３Ａに示す専用のハードウェアの処理回路１０３である場合、処理回路１０３は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）またはこれらを組み合わせたものが該当する。
位置姿勢推定装置１における、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０の各機能をそれぞれ処理回路で実現してもよいし、各機能をまとめて１つの処理回路で実現してもよい。

処理回路が図３Ｂに示すＣＰＵ１０４である場合、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０の各機能は、ソフトウェア、ファームウェア、またはソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェアおよびファームウェアはプログラムとして記述され、メモリ１０５に格納される。

ＣＰＵ１０４は、メモリ１０５に格納されたプログラムを読み出し実行することにより各機能を実現する。すなわち、位置姿勢推定装置１は、ＣＰＵ１０４によって実行されるときに、図４に示すステップＳＴ１からステップＳＴ９までの処理が結果的に実行されるプログラムを格納するためのメモリ１０５を備える。
これらのプログラムは、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０の手順または方法をコンピュータに実行させるものである。

メモリは、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ ＥＰＲＯＭ）などの不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）などが該当する。

また、距離標認識部５、方位推定部６、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０の各機能について、一部を専用のハードウェアで実現し、一部をソフトウェアまたはファームウェアで実現してもよい。
例えば、距離標認識部５および方位推定部６は、専用のハードウェアの処理回路でその機能を実現し、距離標検索部７、方位算出部８、距離標特定部９および位置姿勢推定部１０については、ＣＰＵ１０４がメモリ１０５に格納されたプログラム実行することにより、その機能を実現する。
このように、処理回路は、ハードウェア、ソフトウェア、ファームウェアまたはこれらの組み合わせによって前述の機能を実現することができる。

次に動作について説明する。
図４は、位置姿勢推定装置１の動作を示すフローチャートであり、車両前方の撮影画像を得てから撮影装置２の位置および姿勢が推定されるまでの一連の処理を示している。
まず、距離標認識部５は、撮影装置２から車両前方の撮影画像を入力すると（ステップＳＴ１）、当該撮影画像から距離標の認識を行う（ステップＳＴ２）。
図５は、撮影画像２Ａから距離標４０１を認識する処理の概要を示す図である。図５において、撮影画像２Ａの左上隅の画素が原点（０，０）であり、原点から右方向に延びる軸をｘ軸、上記原点から下方向に延びる軸をｙ軸と定義している。撮影画像２Ａ上の各点は、画素（ピクセル）単位で定義されて画像座標系における位置が表現される。
ただし、前述した原点および座標軸の定義は、あくまで一例であり、これに限定されるものではない。

距離標認識部５は、撮影画像２Ａを画像処理することにより、撮影画像２Ａに写っている距離標４０１を認識すると（ステップＳＴ３；ＹＥＳ）、距離標４０１に標示されているキロ数を認識して距離標検索部７に出力する。図５では、“１２３．４”という数字の標示情報が距離標検索部７に出力される。
距離標認識部５は、画像座標系における距離標４０１の四隅の点の位置座標を認識して位置姿勢推定部１０に出力する。距離標４０１の四隅の点は、距離標４０１におけるキロ数が標示された面上の点であり、図５に示す点４０２ａ，４０２ｂ，４０２ｃ，４０２ｄである。
一方、撮影画像に距離標が写っていない場合、距離標認識部５は、撮影画像から距離標を認識できないため（ステップＳＴ３；ＮＯ）、ステップＳＴ１の処理に戻る。

距離標検索部７は、距離標認識部５によって撮影画像２Ａから認識されたキロ数に対応する距離標を距離標データベース４から検索する（ステップＳＴ４）。
例えば、距離標検索部７が、距離標認識部５から入力したキロ数である“１２３．４”に基づいて、図２に示した距離標データベース４を検索すると、キロ数“１２３．４”に対応する２つの距離標の情報が読み出されて方位算出部８に出力される。前述した距離標の情報とは、距離標の四隅の各点の世界座標系における位置座標である。

次に、方位算出部８は、距離標検索部７から入力した距離標の四隅の各点の世界座標系における位置座標に基づいて、撮影装置２に対する上記距離標の方位を算出する（ステップＳＴ５）。前述のように、撮影画像２Ａから認識されたキロ数“１２３．４”に対応する２つの距離標が距離標データベース４から検索された場合、方位算出部８は、これらの距離標のそれぞれについて方位を算出する。方位算出部８は、算出した距離標ごとの方位と、方位を算出した距離標の四隅の各点（基準点）の世界座標系における位置座標とを、距離標特定部９に出力する。

図６は距離標６０１の方位を算出する処理の概要を示す図であり、距離標６０１を上空から見た様子を示している。図６において、ｘ軸の正方向が東方向であり、ｙ軸の正方向が北方向である。点６０２は、距離標６０１の左上隅の点であり、点６０３は、距離標６０１の右上隅の点である。点６０２，６０３は、前述した基準点である。矢印６０４は、距離標６０１の設置方向、すなわちキロ数が標示された面の法線方向を示している。

角度６０５は、方位算出部８によって算出される距離標６０１の方位を示す角度である。方位算出部８は、距離標６０１の左上隅の点６０２の位置座標、右上隅の点６０３の位置座標、および逆三角関数を利用して、角度６０５を算出する。
方位算出部８は、距離標検索部７によって検索された全ての距離標について上記方位の算出を行い、算出した方位を距離標特定部９に出力する。
上記説明では、距離標６０１の左上隅の点６０２の位置座標と右上隅の点６０３の位置座標とを用いて距離標６０１の方位を算出したが、これらの代わりに距離標６０１の左下隅の点と右下隅の点の各位置座標を用いて距離標６０１の方位を算出してもよい。また、距離標の左上隅の点、右上隅の点、左下隅の点および右下隅の点の組み合わせを用いて、距離標の方位を算出してもよい。

さらに、方位算出部８が、距離標の左上隅の点から左下隅の点へのベクトルと、左上隅の点から右上隅の点へのベクトルとの外積に基づいて、距離標の方位を算出してもよい。例えば、方位算出部８は、上記外積の値に基づいて、距離標においてキロ数が標示された面に対して垂直なベクトルを算出し、算出したベクトルと逆三角関数とを用いて距離標の方位を算出する。

次に、方位推定部６は、センサ装置３からセンサ値を取得すると（ステップＳＴ６）、取得したセンサ値に基づいて撮影装置２の撮影方位を推定する（ステップＳＴ７）。
例えば、方位推定部６は、加速度センサが検出した重力加速度に基づいて、撮影装置２の地面に対する姿勢を算出して、算出した姿勢と地磁気センサが検出した地磁気の値とに基づいて、撮影装置２の撮影方位を推定する。
また、撮影装置２が地面に対して垂直な姿勢（撮影方向は地面に水平）である場合もしくは上記車両に撮影装置２の姿勢が固定されて既知である場合、方位推定部６は、地磁気センサのセンサ情報のみを利用して撮影装置２の撮影方位を推定してもよい。

続いて、距離標特定部９は、方位算出部８が算出した距離標の方位と方位推定部６が推定した撮影装置２の撮影方位とに基づいて、距離標検索部７が検索した距離標の中から、撮影画像における距離標を特定する（ステップＳＴ８）。
図７は、距離標の特定処理の概要を示す図である。図７において、撮影方位７０１は、方位推定部６によって推定された撮影装置２の撮影方位であり、方位角が６０度である。破線で示す方位７０２と方位７０３は、方位算出部８によって算出された上記２つの距離標の方位である。方位７０２の方位角は５０度であり、方位７０３の方位角は２６０度である。

距離標特定部９は、距離標の方位との比較を容易にするため、撮影方位７０１の方位角を１８０度反転させる。図７において、方位７０４は、撮影方位７０１の方位角を１８０度反転させた方位であり、方位角が２４０度となる。
次に、距離標特定部９は、撮影方位７０１を１８０度反転させた方位７０４と、２つの距離標の方位７０２，７０３とをそれぞれ比較する。

例えば、距離標特定部９は、方位７０４の方位角と方位７０２の方位角との差の絶対値７０５を算出し、方位７０４の方位角と方位７０３の方位角との差の絶対値７０６を算出して、絶対値７０５と絶対値７０６とを大小比較する。距離標特定部９は、上記方位角の差の絶対値が最も小さい距離標を、撮影装置２が撮影している距離標、すなわち撮影画像における距離標として特定する。絶対値７０５は１７０度、絶対値７０６は２０度であることから、方位７０３の距離標が、撮影画像における距離標として特定される。
距離標特定部９は、特定した距離標の四隅の各点（基準点）の世界座標系における位置座標を、位置姿勢推定部１０に出力する。

上り線と下り線とに車線が分離された道路において、下り線の始点と上り線の終点とを起点として１００メートルごとに距離標が設置されている場合、距離標には、道路に設定された起点から当該距離標までの距離であるキロ数が標示される。
この場合、図８に示すように、上り線側の距離標８００ａとこれに対応する位置の下り線側の距離標８００ｂとには同じキロ数が標示され、上り線側の距離標８０１ａとこれに対応する位置の下り線側の距離標８０１ｂとには同じキロ数が標示される。

従来の技術では、撮影画像から認識されたキロ数に基づいて、撮影されている距離標を特定するため、例えば、上り線側の距離標８００ａを特定すべきところ、これに対応する位置にある下り線側の距離標８００ｂが誤って特定される可能性があった。
これに対し、距離標特定部９は、方位算出部８が算出した距離標の方位と方位推定部６が推定した撮影装置２の撮影方位とに基づいて、距離標検索部７が検索した距離標の中から、撮影画像における距離標を特定する。これにより、実際に撮影画像に写っている上り線側の距離標８００ａを正確に特定することができる。

位置姿勢推定部１０は、距離標特定部９によって特定された距離標の四隅の各点の世界座標系における位置座標と、当該距離標の四隅の各点の画像座標系における位置座標との対応関係に基づいて、撮影装置２の位置および姿勢を推定する（ステップＳＴ９）。
距離標特定部９が特定した距離標は、撮影装置２によって撮影されている距離標であるので、位置姿勢推定部１０は、当該距離標の四隅の各点の画像座標系における位置座標を距離標認識部５から取得することができる。
位置姿勢推定部１０は、上記距離標の四隅の各点の世界座標系における位置座標と画像座標系における位置座標との対応関係に基づいて、撮影装置２の上記内部パラメータを用いたＰｎＰ問題を解くことで、撮影装置２の回転成分も含む位置および姿勢を推定する。

位置姿勢推定部１０は、位置姿勢推定装置１を搭載する車両と撮影装置２との位置関係が既知である場合、推定した撮影装置２の位置および姿勢と上記位置関係とに基づいて、上記車両の位置および姿勢を正確に推定することができる。車両位置を正確に推定できることから、ナビゲーション精度を高めることもできる。

図４に示した処理のうち、ステップＳＴ１からステップＳＴ５までの処理と、ステップＳＴ６からステップＳＴ７までの処理との間には依存関係が存在しないため、前者の処理と後者の処理とを並行して実行してもよい。

これまで、認識対象物が距離標であり、標示情報がキロ数である場合を示したが、実施の形態１は、これに限定されるものではない。
例えば、認識対象物は、道路沿いに設置された設備であってもよく、標示情報は、この設備に標示された、数字、文字列、画像、２次元バーコードといった符号情報であってもよい。すなわち、認識対象物および標示情報は、撮影画像から画像認識可能なものであればよい。

距離標の基準点が四隅の各点である場合を示したが、これに限定されるものではない。例えば、基準点は、距離標の端辺上の点またはキロ数の標示面上の点であってもよい。
さらに、位置姿勢推定装置１を車両に搭載した場合を示したが、これに限定されるものではない。例えば、位置姿勢推定装置１を、鉄道または航空機などの移動体に搭載してもよく、スマートフォン、タブレットＰＣなどの携帯端末で位置姿勢推定装置１を実現して人が携帯してもよい。

以上のように、実施の形態１に係る位置姿勢推定装置１は、撮影画像から認識されたキロ数に対応する距離標の基準点の位置座標を距離標データベース４から検索し、検索した距離標の基準点の位置座標に基づいて当該距離標の方位を算出し、算出した距離標の方位と撮影装置２の撮影方位とに基づいて上記撮影画像における距離標を特定し、特定した上記距離標の基準点の世界座標系における位置座標と画像座標系における位置座標との対応関係に基づいて撮影装置２の位置および姿勢を推定する。
このように撮影装置２の撮影方位と距離標の方位とに基づくことで、同じキロ数を標示する距離標の中から、撮影画像における距離標を正確に特定することができる。
正確に特定された距離標の基準点の位置座標に基づいて撮影装置２の位置および姿勢を精度よく推定することができる。
撮影された距離標の位置も正確に特定できることから、位置姿勢推定装置１は、道路の保守点検作業の記録に適用することができる。
また、撮影装置２の位置および姿勢に基づいて、位置姿勢推定装置１の位置および姿勢を正確に推定できるので、対象物の正確な位置と姿勢を必要とする拡張現実（ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ）アプリケーションにも適用することができる。

実施の形態１に係る位置姿勢推定装置１において、方位推定部６が、加速度センサの検出情報に基づいて撮影装置２の地面に対する姿勢を推定し、推定した姿勢に基づいて撮影装置２の撮影方位を推定する。このように構成することにより、地面に対して自由な姿勢で取り付けられた撮影装置２の撮影方位を推定することができる。

なお、本発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。

この発明に係る位置姿勢推定装置は、推定対象物の位置および姿勢の推定精度を高めることができるので、例えば、車載用のナビゲーション装置に好適である。

１ 位置姿勢推定装置、２，１００ 撮影装置、２Ａ 撮影画像、３，１０１ センサ装置、４ 距離標データベース、５ 距離標認識部、６ 方位推定部、７ 距離標検索部、８ 方位算出部、９ 距離標特定部、１０ 位置姿勢推定部、１０２ 記憶装置、１０３ 処理回路、１０４ ＣＰＵ、１０５ メモリ、４０１，６０１ 距離標、６０４ 矢印、６０５ 角度、７０１ 撮影方位、７０２〜７０４ 方位、７０５，７０６ 絶対値、８００ａ，８００ｂ，８０１ａ，８０１ｂ 距離標。

Claims (6)

1. 認識対象物に設定された基準点の３次元空間における位置座標と当該認識対象物が標示する標示情報とが対応付けられたデータベースから、撮影画像から認識された標示情報に対応する前記認識対象物を検索する対象物検索部と、
   前記対象物検索部によって検索された前記認識対象物に設定された前記基準点の位置座標に基づいて、前記認識対象物の方位を算出する方位算出部と、
   前記撮影画像を撮影した推定対象物の撮影方位と前記方位算出部によって算出された前記認識対象物の方位とに基づいて、前記対象物検索部によって検索された前記認識対象物の中から、前記撮影画像における前記認識対象物を特定する対象物特定部と、
   前記対象物特定部によって特定された前記認識対象物に設定された前記基準点の３次元空間における位置座標と前記撮影画像における位置座標との対応関係に基づいて、前記推定対象物の位置および姿勢を推定する位置姿勢推定部と
   を備えたことを特徴とする位置姿勢推定装置。
2. 前記推定対象物によって撮影された前記撮影画像から標示情報と前記撮影画像における前記基準点の位置座標とを認識する対象物認識部を備えたこと
   を特徴とする請求項１記載の位置姿勢推定装置。
3. 前記推定対象物の撮影方位を推定する方位推定部を備えたこと
   を特徴とする請求項１記載の位置姿勢推定装置。
4. 前記方位推定部は、加速度センサの検出情報に基づいて前記推定対象物の地面に対する姿勢を推定し、推定した姿勢に基づいて前記推定対象物の撮影方位を推定すること
   を特徴とする請求項３記載の位置姿勢推定装置。
5. 前記基準点は、前記認識対象物において標示情報が標示された面上の複数の点であること
   を特徴とする請求項１記載の位置姿勢推定装置。
6. 対象物認識部が、推定対象物によって撮影された撮影画像から、認識対象物が標示する標示情報と前記認識対象物に設定された基準点の前記撮影画像における位置座標とを認識するステップと、
   対象物検索部が、前記認識対象物に設定された前記基準点の３次元空間における位置座標と前記認識対象物が標示する標示情報とが対応付けられたデータベースから、前記対象物認識部によって前記撮影画像から認識された標示情報に対応する前記認識対象物を検索するステップと、
   方位算出部が、前記対象物検索部によって検索された前記認識対象物に設定された前記基準点の位置座標に基づいて、前記認識対象物の方位を算出するステップと、
   方位推定部が、前記推定対象物の撮影方位を推定するステップと、
   対象物特定部が、前記方位推定部によって推定された前記推定対象物の撮影方位と前記方位算出部によって算出された前記認識対象物の方位とに基づいて、前記対象物検索部によって検索された前記認識対象物の中から、前記撮影画像における前記認識対象物を特定するステップと、
   位置姿勢推定部が、前記対象物特定部によって特定された前記認識対象物に設定された前記基準点の３次元空間における位置座標と前記撮影画像における位置座標との対応関係に基づいて、前記推定対象物の位置および姿勢を推定するステップと
   を備えたことを特徴とする位置姿勢推定方法。

Images