(57)【要約】【課題】移動体へのカメラの設置自由度が高く、かつ移動体標定の高精度化を図ること。【解決手段】仮想カメラＶＣｉｊの位置Ｖijと移動体原点Ｃijとの相対位置関係１）〜３）より、移動体原点Ｃijが存在する位置候補となる移動体原点軌道を特定する。１）移動体原点位置Ｃij−仮想カメラＶＣｉｊ間の距離ｄが一定２）移動体座標系（移動体原点Ｏm）からみた仮想カメラＶＣｉｊの方向が一定（仮想カメラＶＣｉｊから見たマーカーＰjと移動体原点位置Ｃijとの間の角度が一定）３）仮想カメラＶＣｉｊがマーカー円Ｅij上に存在（仮想カメラＶＣｉｊから見たマーカーＰi，Ｐj間の角度が一定）  移動体原点軌道Ｔが複数ある場合に各移動体原点軌道Ｔの交点を求めることで移動体Ｍの位置を特定できる。【選択図】図２−１
2008051745.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をコンピュータに実行させる移動体標定プログラムであって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出させる検出工程と、
前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定させるとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定させる特定工程と、
前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定させる標定工程と、
を前記コンピュータに実行させる移動体標定プログラム。
【請求項２】
前記第１および第２のマーカーを望む第１の仮想光学中心と、前記第２および第３のマーカーを望む第２の仮想光学中心とを算出させる仮想光学中心算出工程と、
前記仮想光学中心算出工程によって算出された第１の仮想光学中心からみた前記第１および第２のマーカーの間の第１のマーカー間角度と、前記仮想光学中心算出工程によって算出された第２の仮想光学中心からみた前記第２および第３のマーカーの間の第２のマーカー間角度とを算出させるマーカー間角度算出工程と、を前記コンピュータに実行させ、
前記特定工程は、
前記マーカー間角度算出工程によって算出された第１のマーカー間角度と、前記第１の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第１の仮想光学中心からの前記第２のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第１の移動体原点軌道を特定させるとともに、
前記マーカー間角度算出工程によって算出された第２のマーカー間角度と、前記第２の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第２の仮想光学中心からの前記第３のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第２の移動体原点軌道を特定させることを特徴とする請求項１に記載の移動体標定プログラム。
【請求項３】
請求項１または２に記載の移動体標定プログラムを記録した前記コンピュータに読み取り可能な記録媒体。
【請求項４】
移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定装置であって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出手段と、
前記検出手段によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出手段によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定手段と、
前記未知の移動体原点位置を、前記特定手段によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定手段と、
を備えることを特徴とする移動体標定装置。
【請求項５】
移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定方法であって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出工程と、
前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定工程と、
前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定工程と、
を含んだことを特徴とする移動体標定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法に関する。
【背景技術】
【０００２】
移動体の位置を計測する手段としてはＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）が広く用いられている。ＧＰＳ単独測位は１０［ｍ］程度の精度があり、ドライバーへの経路提示といったナビゲーションサービスのための測位技術としては実用的ではあるが、介入制御、自動走行などの高度な車両コントロールのための測位としては精度が足りない。
【０００３】
また、ＧＰＳ干渉測位方式（ＲＴＫ，ＶＲＳ−ＲＴＫ等）を用いると数［ｃｍ］の測位精度を実現できるが、機器が非常に高価であること、反射波により精度が悪化すること、トンネルなどで電波が遮断された後の復帰に非常に時間がかかることなどから路上で利用するには問題が多い。
【０００４】
一方、移動体に搭載したカメラにより複数のマーカーを撮影し、マーカーどうしが成す角度から位置標定をおこなう手法がある。図１６は、移動体に搭載したカメラにより撮影されたマーカーどうしが成す角度から位置標定をおこなう手法を示す説明図である。
【０００５】
図１６において、複数のマーカーＰ1，Ｐ2，Ｐ3，・・・の位置が既知である場合、観測された２つのマーカーＰi，Ｐj（ｉ，ｊ＝１，２，３，・・・、ただしｉ≠ｊ）と移動体Ｍは、下記式（１．１）であらわされる中心位置Ｏijと下記式（１．２）であらわされる半径ｒijとを持つ円（マーカー円Ｅij）の円周上に存在する。
【０００６】
【数１】
2008051745.tif 000003
【０００７】
上記式（１．２）において、θijは、２つのマーカーＰi，Ｐjの成すマーカー間角度である。θijは、移動体原点Ｏmから撮影された撮影画像により算出される。３個以上のマーカーが検知された場合、複数のマーカー円Ｅijを式（１．１）および式（１．２）により得ることができるため、これらのマーカー円Ｅij（図１６ではＥ12，Ｅ23）の交点位置を求めることで連続的に移動体位置を得ることができる。
【０００８】
上記方式は精度高く移動体位置を求めることができるものの、移動体原点Ｏmを基準とした水平３６０度を計測範囲とするマーカー方位計測手段が必要になる。そのため、移動体原点Ｏmに光学中心を持つ特殊な全方位カメラを用いることが必要である。
【０００９】
図１７は、全方位カメラの光学系および当該光学系により撮像される画像例を示す説明図である。図１７において、投影面ｆは光学中心を通る円直線を中心軸Ｙとする円柱の側面により構成されている。撮影された画像は、水平軸を方位角θと対応して記録される。このためマーカー投影像の水平画素位置から容易にマーカー方位角とマーカー間角度（図１７ではθ12，θ23）を算出することができる。
【００１０】
このような全方位カメラを実現するために、たとえば、下記特許文献１では、光学中心を軸に３６０度回転させる機構を持たせており、また、下記特許文献２では、特殊な反射板とレンズからなる光学系を用いている。
【００１１】
また、上述のような全方位カメラを用いずに、複数カメラから３６０度視野のパノラマ撮影画像を作成する方式及び商品が各種存在している。図１８は、複数のカメラＣＭ１〜ＣＭ３を自動車に設置した場合の撮像例を示す説明図である。図１８において、各カメラＣＭ１〜ＣＭ３はその視界内のマーカーＰ1，Ｐ2，Ｐ3，・・・を撮像することができる。これらカメラＣＭ１〜ＣＭ３を複数組み合わせることで、周辺視野を生成することができる（たとえば、下記特許文献３を参照。）。
【００１２】
【特許文献１】特開平１１−８３４８０号公報
【特許文献２】特開２０００−３３７８８７号公報
【特許文献３】特開２００６−５４６６２号公報
【発明の開示】
【発明が解決しようとする課題】
【００１３】
しかしながら、上述した全方位カメラを用いた従来技術では、いずれも機構的に複雑であり高価かつ大型になる。また、移動体に設置する場合には全方位カメラを３６０度の視界が開けている場所に設置する必要があるが、自動車などの移動体では、一般的にはその頭頂部しか設置することができず、設置自由度はきわめて低い。特に、自動車についてはその外形デザイン上が重視されるため、極めて重大な制約を課すことになるという問題があった。
【００１４】
また、複数のカメラを用いた従来技術では、各カメラＣＭ１〜ＣＭ３の光学中心のズレを積極的に解決していない。たとえば、図１８に示した例において、カメラＣＭ１〜ＣＭ３で撮像されたマーカーＰ1，Ｐ2，Ｐ3は、カメラＣＭ１〜ＣＭ３の独自の座標系によりマーカーＰ1，Ｐ2，Ｐ3の方向を計測することが可能（図１８中、○で表示）であるが、移動体原点Ｏmを光学中心とした場合のマーカーＰ1，Ｐ2，Ｐ3の方向を計測することは、カメラＣＭ１〜ＣＭ３の光学中心のズレにより不可能ないしは可能であったとしても精度が悪いという問題があった（図１８中、×で表示）。
【００１５】
この発明は、上述した従来技術による問題点を解消するため、移動体へのカメラの設置自由度が高く、かつ、移動体の位置標定の高精度化を図ることができる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法を提供することを目的とする。
【課題を解決するための手段】
【００１６】
上述した課題を解決し、目的を達成するため、この発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法は、移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法であって、前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出し、位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定し、前記未知の移動体原点位置を、特定された第１および第２の移動体原点軌道の交点座標に標定することを特徴とする。
【００１７】
また、上記発明において、前記第１および第２のマーカーを望む第１の仮想光学中心と、前記第２および第３のマーカーを望む第２の仮想光学中心とを算出し、算出された第１の仮想光学中心からみた前記第１および第２のマーカーの間の第１のマーカー間角度と、算出された第２の仮想光学中心からみた前記第２および第３のマーカーの間の第２のマーカー間角度とを算出し、算出された第１のマーカー間角度と、前記第１の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第１の仮想光学中心からの前記第２のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第１の移動体原点軌道を特定するとともに、算出された第２のマーカー間角度と、前記第２の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第２の仮想光学中心からの前記第３のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第２の移動体原点軌道を特定することとしてもよい。
【００１８】
また、上記発明において、前記第１の移動体原点軌道を表現する関数が、第１の変数を用いて定義され、かつ、前記第２の移動体原点軌道を表現する関数が、第２の変数を用いて定義されている場合、前記第１の移動体原点軌道を表現する関数の値と第２の移動体原点軌道を表現する関数の値との差分を数値解析で評価する評価関数が最小となるように、前記第１および第２の変数の値を決定し、決定された前記第１の変数の値を前記第１の移動体原点軌道関数に与え、かつ、決定された前記第２の変数の値を前記第２の移動体原点軌道関数に与えることにより、前記未知の移動体原点位置を、前記第１および第２の移動体原点軌道の交点座標に標定することとしてもよい。
【００１９】
また、上記発明において、前記第１および第２のマーカーと前記第１の仮想光学中心とが同一円周上に位置する第１のマーカー円を表現する関数が前記第１の変数を用いて定義され、前記第２および第３のマーカーと前記第２の仮想光学中心とが同一円周上に位置する第２のマーカー円を表現する関数が前記第２の変数を用いて定義されている場合、前記第１の変数の値を前記第１のマーカー円を表現する関数に与え、かつ、前記第２の変数の値を前記第２のマーカー円を表現する関数に与えることにより、前記移動体の方位角を標定することとしてもよい。
【００２０】
また、上記発明において、前記第１〜第３のマーカーの位置および方向が、前記移動体に搭載されているＧＰＳおよび電子コンパスによって取得された前記移動体の位置および方位を用いて検出された場合、標定された移動体原点位置および前記移動体の方位角を用いて、移動体標定をおこなうこととしてもよい。
【００２１】
この発明によれば、移動体標定（位置や方位角の標定）を移動体に搭載された複数のカメラを用いて実現することができる。また、すでに複数のカメラが搭載済みの移動体については、本発明を取り入れるだけで、移動体標定をおこなうことができる。また、複数のカメラが未搭載の移動体についても、全方位カメラのような設置位置の制約を受けずに複数のカメラを設置することができ、移動体標定を移動体のデザインを損なうことなく高精度に実現することができる。また、ＧＰＳや電子コンパスなどの低精度の移動体標定データを用いて本発明を繰り返し実行することで、移動体標定を高精度に算出することができる。
【発明の効果】
【００２２】
本発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法によれば、移動体へのカメラの設置自由度が高く、かつ、移動体標定の高精度化を図ることができるという効果を奏する。
【発明を実施するための最良の形態】
【００２３】
以下に添付図面を参照して、この発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法の好適な実施の形態を詳細に説明する。本実施の形態では全方位カメラでマーカーを検知するかわりに、複数の一般的なカメラを組み合わせることでマーカーを検知する。自動車に関しては既に自動車周辺の視界確保用に前方、後側方、後方に複数台のカメラを装備しているものもあるため、これらを利用してもよい。
【００２４】
（移動体標定の原理）
まず、この発明の実施の形態にかかる移動体標定の原理について説明する。図１は、この発明の実施の形態にかかる移動体標定の原理を示す説明図である。この移動体標定では、移動体Ｍに搭載された異なるカメラＣＭｉ，ＣＭｊ（ｉ，ｊ＝１，２，３，・・・、ただしｉ≠ｊ）で撮影される２つのマーカーＰi，Ｐjを同時に撮影できる位置に仮想的にカメラ（の光学中心）があるものと考える。このカメラを「仮想カメラＶＣｉｊ」と称す。仮想カメラＶＣｉｊの設置位置をＶijと表記する。
【００２５】
また、以降、仮想カメラＶＣｉｊと対比するため、移動体Ｍに搭載されたカメラＣＭｉを「実カメラＣＭｉ」と称する場合もある。なお、「マーカー」とは、カメラＣＭｉの撮像目標であり、たとえば、信号機、道路標識、電柱などの設置物、またはこれらに取り付けられ周囲から識別可能な目印があげられる。
【００２６】
また以下の説明および式では、カメラＣＭｉ，ＣＭｊで撮影される一対のマーカーＰi，Ｐjを用いて説明するが、カメラＣＭｊ，ＣＭｋで撮影される一対のマーカーＰj，Ｐkについては、「ｉ」を「ｊ」に置き換えるとともに、「ｊ」を「ｋ」に置き換えることとし、その説明を省略する。
【００２７】
図１において、仮想カメラＶＣｉｊは、移動体Ｍに搭載された異なるカメラＣＭｉ，ＣＭｊで撮影される２つのマーカーＰi，Ｐjを同時に撮影できる位置に仮想設置されている。すなわち、仮想カメラＶＣｉｊは、カメラＣＭｉとマーカーＰiとを通過する線分と、カメラＣＭｊとマーカーＰjとを通過する線分との交点に仮想設置されている。
【００２８】
また、仮想カメラＶＣｊｋは、移動体Ｍに搭載された異なるカメラＣＭｊ，ＣＭｋで撮影される２つのマーカーＰj，Ｐkを同時に撮影できる位置に仮想設置されている。すなわち、仮想カメラＶＣｊｋは、カメラＣＭｊとマーカーＰjとを通過する線分と、カメラＣＭｋとマーカーＰkとを通過する線分との交点に仮想設置されている。
【００２９】
このように、仮想カメラＶＣｉｊはマーカー間角度θijが計算できる位置に設定され、移動体原点Ｏmを光学中心とした移動体座標系上の仮想カメラＶＣｉｊの位置Ｖijは、カメラＣＭｉ，ＣＭｊとマーカーＰi，Ｐjの方向から算出することができる。同様に、仮想カメラＶＣｊｋはマーカー間角度θjkが計算できる位置に設定され、移動体原点Ｏmを光学中心とした移動体座標系上の仮想カメラＶＣｊｋの位置Ｖjkは、カメラＣＭｊ，ＣＭｋとマーカーＰj，Ｐkの方向から算出することができる。
【００３０】
また、上述した［背景技術］では、マーカーＰi，Ｐjと移動体原点Ｏmを通る円をマーカー円Ｅijと称したが、本実施の形態では、マーカーＰi，Ｐjと仮想カメラＶＣｉｊの設置位置Ｖijとを通る中心位置Ｏij、半径ｒijの円を、マーカー円Ｅijと称す。マーカー円Ｅijは、仮想カメラＶＣｉｊから見たマーカー間角度θijを上述した式（１．１），（１．２）に代入することで、中心位置Ｏijと半径ｒijとを持つ円（図１では、ＥijとＥjk）を特定することができる。
【００３１】
つぎに、仮想カメラＶＣｉｊの設置位置Ｖijと移動体原点位置Ｃijとの相対位置関係より、移動体原点位置Ｃijが存在する位置候補となる移動体原点軌道を特定する。図２−１は、この発明の実施の形態にかかる移動体原点軌道を示す説明図である。移動体原点軌道Ｔは、以下の１）〜３）の条件を満たす位置座標の集合である。
【００３２】
１）移動体原点位置Ｃij−仮想カメラＶＣｉｊ間の距離ｄが一定
２）移動体座標系（移動体原点Ｏm）からみた仮想カメラＶＣｉｊの方向が一定（仮想カメラＶＣｉｊから見たマーカーＰjと移動体原点位置Ｃijとの間の角度が一定）
３）仮想カメラＶＣｉｊがマーカー円Ｅij上に存在（仮想カメラＶＣｉｊから見たマーカーＰi，Ｐj間の角度が一定）
【００３３】
図２−２は、上記１）〜３）の条件を満たす移動体原点軌道Ｔを示す説明図である。移動体原点軌道Ｔが複数ある場合（たとえば、３点以上のマーカーＰ1，Ｐ2，Ｐ3，・・・が観測された場合）に各移動体原点軌道Ｔの交点を求めることで移動体Ｍの位置を特定できる。各移動体原点軌道Ｔの交点は逐次近似法などの数値解法で求めることができる。
【００３４】
（移動体標定手法）
つぎに、上述した移動体標定の原理を用いた移動体標定の手法について説明する。図３は、この発明の実施の形態にかかる各種座標系を示す説明図である。図３において、移動体座標系３０１は、移動体Ｍの進行方向を示すＹm軸と、Ｙm軸と直交するＸm軸とで水平面をあらわす座標系である。
【００３５】
カメラ座標系３０２は、カメラ原点Ｏcを各カメラＣＭｉの光学中心とし、カメラＣＭｉの光軸方向を示すＹci軸と、Ｙci軸と直交するＸci軸とで水平面をあらわす座標系である。カメラ座標系３０２は、カメラＣＭｉの個数分存在する。また、世界座標系３０３は、所定の基準点を原点Ｏwとし、南北方向を示すＹw軸と、東西方向を示すＸw軸とで水平面をあらわす座標系である。各座標系の鉛直方向を示すＺ軸（不図示）は図３の紙面を突き刺す方向である。各座標系３０１〜３０３間の縮尺変化はなくＺ軸まわりの回転と並行移動で座標系変換が可能とする。なお、図３中、Ｔcm，Ｔmwは座標間移動距離である。
【００３６】
図４は、任意のカメラＣＭｉによって１つのマーカーＰiが撮影された場合のマーカー方位推定を示す説明図である。マーカーＰiはカメラＣＭｉが撮影した画像座標系４００（Ｘｐ−Ｙｐ）中で（ｘp，ｙp）の位置に撮像されたとすると、カメラ座標系３０２上では光軸方向（Ｙc軸）から角度α回転した方向にマーカーＰiが存在する。このときαは下記式（２）であらわされる。
【００３７】
【数２】
2008051745.tif 000004
【００３８】
ここで、ここでΘHはカメラＣＭｉの水平画角でありＷpは画像の幅を示している。カメラ座標系３０２から移動体座標系３０１への変換が（ｔx，ｔy）の水平移動と角度βの回転移動であるとき、移動体座標系３０１におけるカメラＣＭｉのカメラ原点（光学中心）ＯciとマーカーＰiを通る直線（マーカーライン）Ｌiは下記式（３）であらわされる。
【００３９】
【数３】
2008051745.tif 000005
【００４０】
同様に他のカメラとマーカーについてもそのカメラ原点（光学中心）とマーカーを通る直線（マーカーライン）が上記式（３）により算出されることとなる。
【００４１】
図５は、マーカー間角度推定を示す説明図である。図５では、複数のカメラ（ＣＭｉ，ＣＭｊ）から得られたマーカーラインＬi，Ｌjを用いてマーカー間角度θijを推定する。具体的には、各マーカーラインＬi，Ｌjどうしの交点を求め、その交点を基準としたマーカーＰi，Ｐjの成す角度（マーカー間角度θij）を算出する。任意の２マーカーＰi，Ｐjを同一のカメラＣＭｉで撮影した場合はマーカーラインＬi，Ｌjの交点はそのカメラＣＭｉのカメラ原点（光学中心）Ｏciと一致する。
【００４２】
一方、異なるカメラＣＭｉ，ＣＭｊで撮影されている場合は、点Ｑijが交点となる。この交点Ｑijを「仮想光学中心Ｑij」と称す。仮想光学中心Ｑijは、下記式（４．１）〜式（４．５）であらわされる。
【００４３】
【数４】
2008051745.tif 000006
【００４４】
また、仮想光学中心Ｑijから見た２つのマーカーＰi，Ｐjのマーカー間角度θij*は、下記式（５）であらわされる。
【００４５】
【数５】
2008051745.tif 000007
【００４６】
また、仮想光学中心Ｑijから見た一方のマーカーＰiと移動体原点Ｏmと間の角度φimは、下記式（６．１）〜式（６．３）であらわされ、仮想光学中心Ｑijから見た他方のマーカーＰjと移動体原点Ｏmと間の角度φjmは、下記式（６．２），式（６．４），式（６．５）であらわされる。
【００４７】
【数６】
2008051745.tif 000008
【００４８】
なお、図５中、カメラ原点ＯcjはカメラＣＭｊのカメラ原点（光学中心）であり、Ｙci軸は、カメラＣＭｊの光軸方向を示すカメラ座標系の座標軸であり、Ｘcj軸は、Ｙcj軸と直交することで水平面をあらわすカメラＣＭｊのカメラ座標系の座標軸である。
【００４９】
図６は、マーカー間角度θij*からの移動体原点Ｏmの世界座標系３０３における推定を示す説明図である。以降、各記号において、「*」が付されている記号は、世界座標系３０３における値（座標位置または角度）を示している。たとえば、マーカーＰi*は、マーカーＰiの世界座標系３０３における座標位置を示している。図６において、マーカー間角度θij*から推定される仮想光学中心Ｑijの世界座標系３０３における位置は、中心位置Ｏij*，半径ｒij*の円周上の点Ｑij*である。中心位置Ｏij*，半径ｒij*は、下記式（７．１），式（７．２）であらわされる。
【００５０】
【数７】
2008051745.tif 000009
【００５１】
ただし、Ｐi*＝（ｐix*，ｐiy*），Ｐj*＝（ｐjx*，ｐjy*）は世界座標系３
０３におけるマーカー位置座標である。
【００５２】
移動体原点Ｏmと仮想光学中心Ｑij*間の距離｜Ｑij｜と、移動体原点ＯmとマーカーＰiとの成す角度φimは、移動体座標系３０１でも世界座標系３０３でも共通であるため、移動体Ｍの位置Ｃij*は下記式（８）を満たす。
【００５３】
【数８】
2008051745.tif 000010
【００５４】
ここで、移動体Ｍの位置Ｃij*は、上記式（７．１）〜（７．２）で示される円周上の１点であるとしかわかっておらず、式（８）だけでは移動体Ｍの位置は特定できない。仮に仮想光学中心Ｑij*を変数μijを用いて極座標系であらわすと下記式（９）のようになる。
【００５５】
【数９】
2008051745.tif 000011
【００５６】
移動体Ｍの位置Ｃij*は変数μijの関数として下記式（１０）であらわされる。
【００５７】
【数１０】
2008051745.tif 000012
【００５８】
式（１０）では、任意の一対のマーカーＰi，Ｐjに対して計算することができる。たとえば同様に一対のマーカーＰj，Ｐkに対しても式（１０）を計算すると、その位置Ｃjk*はＣij*と同じ位置を指し示しているから、下記式（１１）のＦ（μij，μjk）の値を０とする変数μijおよびμjkを求め、この変数μijおよびμjkを式（１０）に代入することで世界座標系３０３における移動体位置Ｃij*を特定することができる。
【００５９】
【数１１】
2008051745.tif 000013
【００６０】
また、移動体Ｍの姿勢を移動体座標系３０１のＹm軸方向の指し示す方位角ω（μij）は、下記式（１２．１），式（１２．２）で特定することができる。
【００６１】
【数１２】
2008051745.tif 000014
【００６２】
なお、変数μijおよびμjkを求めるには解析的におこなってもよいし数値解法を用いてもよい。数値解法を用いる場合、結果は初期値に依存するが、移動体位置の初期値として連続的な計測の場合は前回の位置を用いたり、他の移動体標定手段から取得した値を用いることで良好な初期値を得ることができる。後者の場合、他の移動体標定手段の精度は低くても良く、たとえば、従来のＧＰＳや電子コンパスによる移動体標定などを利用できる。
【００６３】
ここで、上述したＧＰＳの計測値からの変数μijの初期値の算出手法について説明する。図７は、ＧＰＳの計測値からの変数μijの初期値の算出手法を示す説明図である。図７において、ＧＰＳにより計測された緯度経度を世界座標系３０３（直交座標系）に変換した位置をＧij*＝（ｇijx，ｇijy）とし、仮想光学中心座標をＲij*とする。
【００６４】
このときＲij*の座標値は未知であるが、その座標値は上記式（７．１），（７．２）で求められた中心位置Ｏij*，半径ｒij*であらわされる円Ｅijの円周上にあることがわかっている。一方、マーカーＰi*と仮想光学中心座標をＲij*と緯度経度座標Ｇij*とが成す角度が角度φim（上記式（６．１）〜式（６．３）を参照）に近似できるとすると、仮想光学中心座標Ｒij*は、下記式（１３．１），式（１３．２）であらわされる円Ｅの円周上に存在する。
【００６５】
【数１３】
2008051745.tif 000015
【００６６】
したがって、上記円Ｅと円Ｅijとの交点を求めることで仮想光学中心座標Ｒij*が求まる。また、仮想光学中心座標Ｒij*は下記式（１４）であらわされる。
【００６７】
【数１４】
2008051745.tif 000016
【００６８】
上記式（１４）において、変数λijを最もよく満たす値を変数μijの初期値として決定することができる。
【００６９】
つぎに、変数μijおよびμjkの数値解法について具体的に説明する。図８は、式（１１）で示した評価関数Ｆ（μij，μjk）の解空間となる曲面を示す説明図である。変数μijおよびμjkはともに［０，２π］の定義域を持ち、Ｆ（μij，μjk）は各変数μij，μjkに対して０以上の一意の値を持つ。この曲面Ｓ中でＦ（μij，μjk）＝０となる（μij，μjk）の値を求めることで、移動体座標原点Ｏmを特定できることになる。
【００７０】
Ｆ（μij，μjk）＝０となる（μij，μjk）の値は、数値解法によって求めることができる。任意の位置（λij，λij）を（μij，μjk）の初期値とすると、Ｆ（μij，μjk）の解空間中で値が減少する方向に（λij，λij）を移動させていき、Ｆが０（もしくは最小）となる位置が、求めたい（μij，μjk）の値となるはずである。
【００７１】
ただし、Ｆ（μij，μjk）の曲面は単調減少とは限らず、よって必ずしも正しい解に集束するとは限らないため、初期値の選び方が重要となる。そこで初期値としては、前回の移動体標定時の（μij，μjk）の値を用いるか、上述したＧＰＳによる粗い位置計測結果から求めた変数μijの初期値を用いることで誤った解への収束がおこらないようにする。
【００７２】
（移動体標定装置のハードウェア構成）
まず、この発明の実施の形態にかかる移動体標定装置のハードウェア構成について説明する。図９は、この発明の実施の形態にかかる移動体標定装置のハードウェア構成を示すブロック図である。
【００７３】
図９において、移動体標定装置は、ＣＰＵ９０１と、ＲＯＭ９０２と、ＲＡＭ９０３と、ＨＤＤ（ハードディスクドライブ）９０４と、ＨＤ（ハードディスク）９０５と、光ディスクドライブ９０６と、着脱可能な記録媒体の一例としての光ディスク９０７と、ディスプレイ９０８と、Ｉ／Ｆ（インターフェース）９０９と、入力キー９１０と、マイク９１１と、スピーカ９１２と、ＧＰＳ９１３と、電子コンパス９１４と、カメラ群ＣＭとを備えている。また、各構成部はバス９００によってそれぞれ接続されている。
【００７４】
ここで、ＣＰＵ９０１は、移動体標定装置の全体の制御を司る。ＲＯＭ９０２は、ブートプログラムなどのプログラムを記憶している。ＲＡＭ９０３は、ＣＰＵ９０１のワークエリアとして使用される。ＨＤＤ９０４は、ＣＰＵ９０１の制御にしたがってＨＤ９０５に対するデータのリード／ライトを制御する。ＨＤ９０５は、ＨＤＤ９０４の制御で書き込まれたデータを記憶する。
【００７５】
光ディスクドライブ９０６は、ＣＰＵ９０１の制御にしたがって光ディスク９０７に対するデータのリード／ライトを制御する。光ディスク９０７は、光ディスクドライブ９０６の制御で書き込まれたデータを記憶したり、光ディスク９０７に記憶されたデータを移動体標定装置に読み取らせたりする。
【００７６】
また、ディスプレイ９０８は、カーソル、アイコンあるいはツールボックスをはじめ、文書、画像、機能情報などのデータを表示する。このディスプレイ９０８は、たとえば、ＣＲＴ、ＴＦＴ液晶ディスプレイ、プラズマディスプレイなどを採用することができる。
【００７７】
Ｉ／Ｆ９０９は、通信回線を通じてインターネットなどのネットワークに接続され、このネットワークを介して他の装置に接続される。そして、Ｉ／Ｆ９０９は、ネットワークと内部のインターフェースを司り、外部装置からのデータの入出力を制御する。Ｉ／Ｆ９０９には、たとえばモデムやＬＡＮアダプタなどを採用することができる。
【００７８】
入力キー９１０は、文字、数字、各種指示などの入力のためのボタンであり、データの入力をおこなう。また、タッチパネル式の入力パッドであってもよい。マイク９１１は外部からの音声を集音しディジタル信号に変換する。スピーカ９１２はディジタル信号から変換された音声信号に基づいて音声出力する。
【００７９】
ＧＰＳ９１３は、ＧＰＳ衛星からの電波を受信するためのアンテナ、受信した電波を復調するチューナーおよび復調した情報に基づいて現在位置を算出する演算回路等によって構成され、ＧＰＳ衛星からの電波を受信し、ＧＰＳ衛星との幾何学的位置を求めることで、移動体Ｍの現在地点を取得することができる。電子コンパス９１４は、移動体Ｍの方角を算出するＩＣであり地磁気センサを内蔵している。地磁気センサにより地球が生じている南北の地磁気を検出して、移動体Ｍの方角を算出する。カメラ群ＣＭは、移動体Ｍに透視された複数のカメラＣＭ１〜ＣＭｎの集合である。
【００８０】
（マーカーＤＢの記憶内容）
つぎに、この発明の実施の形態にかかるマーカーデータベース（ＤＢ）の記憶内容について説明する。図１０は、マーカーＤＢの記憶内容を示す説明図である。図１０において、マーカーＤＢ１０００は、マーカーＰiごとにマーカーＩＤ、マーカー種別、マーカー緯度、マーカー経度、経度方向距離、緯度方向距離とを含むマーカーデータを記憶している。
【００８１】
マーカー種別とは、マーカーＰiの種別を特定する情報であり、たとえば、「０」が基準点、「１」が信号機、「２」が道路標識、「３」が電柱などのように定義されている。また、マーカー緯度とはマーカーＰiの緯度、マーカー経度とはマーカーＰiの経度をあらわしている。
【００８２】
また、経度方向距離とは、マーカーＰiの基準点からの経度方向距離をあらわしており、緯度方向距離とは、マーカーＰiの基準点からの緯度方向距離をあらわしている。このマーカーＤＢ１０００は、具体的には、たとえば、図９に示したＲＯＭ９０２、ＲＡＭ９０３、ＨＤ９０５、光ディスク９０７などの記録媒体によってその機能を実現する。
【００８３】
（移動体標定装置の機能的構成）
つぎに、この発明の実施の形態にかかる移動体標定装置の機能的構成について説明する。図１１は、この発明の実施の形態にかかる移動体標定装置の機能的構成を示すブロック図である。図１１において、移動体標定装置１１００は、マーカーＤＢ１０００と、カメラ群ＣＭ（・・・，ＣＭｉ，ＣＭｊ，ＣＭｋ，・・・）と、画像処理部１１０１（・・・，１１０１ｉ，１１０１ｊ，１１０１ｋ，・・・）と、検出部１１０２と、特定部１１０３と、標定部１１０４と、から構成されている。
【００８４】
各カメラＣＭｉ，ＣＭｊ，ＣＭｋは、移動体Ｍに対して周辺水平方向の道路環境を撮影するように設置されている。各カメラＣＭｉ，ＣＭｊ，ＣＭｋは、カメラ座標系３０２におけるカメラＣＭｉ，ＣＭｊ，ＣＭｋの設置位置座標（ｔix，ｔiy），（ｔjx，ｔjy），（ｔkx，ｔky）や、移動体座標系３０１との座標軸のずれをあらわす回転角度βi，βj
，βkも定まっている。各カメラＣＭｉ，ＣＭｊ，ＣＭｋの設置位置座標（ｔix，ｔiy），（ｔjx，ｔjy），（ｔkx，ｔky）および回転角度βi，βj，βkは、後述する検出部
１１０２に読み込まれる。
【００８５】
また、画像処理部１１０１は、カメラ群ＣＭで撮影された移動体Ｍ外の撮影画像の中からマーカーＰi，Ｐj，Ｐkの画像を抽出する。撮影画像中のマーカーＰi，Ｐj，Ｐk
の画像抽出および認識は各種従来技術が存在するのでそれらを利用することで求められる。一般的には、マーカーＰi，Ｐj，Ｐkが信号機である場合、信号機の発光するレンズ
の色（青、黄、赤）と形状（円形）をもとに画像中で特定することで抽出することができる。なお、画像処理部１１０１は、カメラと１対１で構成されているがカメラ群ＣＭを単一の画像処理部１１０１で処理する構成としてもよい。
【００８６】
また、検出部１１０２は、移動体Ｍ外の各撮影画像にそれぞれ含まれているマーカーＰi，Ｐj，Ｐkの位置および方向を検出する。具体的には、画像処理部１１０１で画像抽
出されたマーカーＰi，Ｐj，Ｐkの水平画角αi，αj，αkを上記式（２）により算
出する。そして、マーカーＤＢ１０００に格納されているマーカーＰi，Ｐj，Ｐkの世
界座標系３０３における位置情報（たとえば、緯度経度、または基準点を設けた上での直交座標値）を参照し、観測されたマーカーＰi，Ｐj，Ｐkに該当するマーカーデータを
マーカーＤＢ１０００の中から抽出する。
【００８７】
移動体Ｍの位置および姿勢（方向）が未知の状態ではマーカーＰi，Ｐj，Ｐkの特定
は困難なため、移動体Ｍの位置としてＧＰＳ９１３の出力結果を、また移動体Ｍの姿勢として電子コンパス９１４の計測値を用いて、抽出されたマーカーＰi，Ｐj，Ｐkの緯度
経度やマーカー種別を推測し、最も近似するマーカーデータをマーカーＤＢ１０００から抽出することで、画像処理されたマーカーの世界座標系３０３での位置を特定する。
【００８８】
また、連続的に位置計測をおこなっている場合は前回、移動体標定装置１１００により計測された自車位置を使用してもよい。仮の自車位置と姿勢を利用する場合、その位置および姿勢に移動体Ｍを配置したときに各カメラＣＭｉ，ＣＭｊ，ＣＭｋの水平画角αi，
αj，αkの方向にあるマーカーを抽出し、抽出されたマーカーＰi，Ｐj，Ｐkのうち
最も角度誤差が小さく距離が近いマーカーＰi，Ｐj，Ｐkを、対応するマーカー（世界
座標系３０３における座標値Ｐi*，Ｐj*，Ｐk*）として抽出する。
【００８９】
抽出されたマーカーＰi*，Ｐj*，Ｐk*のマーカーデータＤi＝｛αi，βi，（
ｔix，ｔiy），Ｐi*｝，Ｄj＝｛αj，βj，（ｔjx，ｔjy），Ｐj*｝，Ｄk＝｛α
k，βk，（ｔkx，ｔky），Ｐk*｝は特定部１１０３に出力される。
【００９０】
また、特定部１１０３は、検出部１１０２により検出されたマーカーＰi*，Ｐj*（
マーカーデータＤi，マーカーデータＤj）に基づいて、未知の移動体原点位置に関する
移動体原点軌道Ｃij*を特定させる。また、マーカーＰj*，Ｐk*（マーカーデータＤj，マーカーデータＤk）に基づいて、未知の移動体原点位置に関する移動体原点軌道Ｃjk*を特定する。ここで、特定部１１０３の詳細な機能的構成については後述する（図１２を参照。）。
【００９１】
また、標定部１１０４は、未知の移動体原点位置を、上記式（１０）および上記式（１１）を用いて、特定部１１０３によって特定された移動体原点軌道Ｃij*，Ｃjk*の交点座標に標定する。また、標定部１１０４は、上記式（１２．１）および上記式（１２．２）を用いて、移動体Ｍの方位角ω（μij）を標定する。すなわち、移動体原点軌道Ｃij*，Ｃjk*の交点座標や移動体Ｍの方位角ω（μij）を出力データ１１１０として出力する。
【００９２】
つぎに、上述した特定部１１０３の詳細な機能的構成について説明する。図１２は、特定部１１０３の詳細な機能的構成を示すブロック図である。特定部１１０３は、仮想光学中心算出部１１３１（・・・，１１３１ｉｊ，１１３１ｊｋ，・・・）と、マーカー間角度算出部１１３２（・・・，１１３２ｉｊ，１１３２ｊｋ，・・・）と、マーカー円算出部１１３３（・・・，１１３３ｉｊ，１１３３ｊｋ，・・・）と、関数決定部１１３４（・・・，１１３４ｉｊ，１１３４ｊｋ，・・・）と、を備えている。
【００９３】
以下、代表として、２つのマーカーデータＤi，Ｄjを取り込んだ場合について説明する。２つのマーカーデータＤi，Ｄjを取り込んだ場合については、機能部の末尾の符号「ｉｊ」を「ｊｋ」に置き換えるだけであるので、その説明を省略する。
【００９４】
まず、仮想光学中心算出部１１３１ｉｊは、２つのマーカーデータＤi，Ｄjを取り込んで、上記式（４．１）〜（４．５）を用いて仮想光学中心Ｑijを算出する。マーカー間角度算出部１１３２ｉｊは、仮想光学中心算出部１１３１ｉｊによって算出された仮想光学中心Ｑijから見た２つのマーカーＰi，Ｐjのマーカー間角度θij*を、上記式（５）を用いて算出する。
【００９５】
また、マーカー円算出部１１３３ｉｊは、マーカー間角度算出部１１３２ｉｊによって算出されたマーカー間角度θij*から推定される仮想光学中心Ｑijの世界座標系３０３における位置を特定するマーカー円Ｅijの中心位置Ｏij*，半径ｒij*を、上記式（７．１），式（７．２）を用いて算出する。
【００９６】
また、関数決定部１１３４ｉｊは、上記式（８）および式（９）を用いて、世界座標系３０３における移動体Ｍの原点軌道関数Ｃij*（μij）（上記式（１０）を参照。）を決定する。このように、関数決定部１１３４では、マーカー対（Ｐi，Ｐj），（Ｐj，Ｐk）ごとに、原点軌道関数Ｃij*（μij），Ｃjk*（μjk）を決定する。
【００９７】
この場合、標定部１１０４では、一のマーカー対（Ｐi，Ｐj）から算出された原点軌道関数Ｃjk*（μjk）と、他のマーカー対（Ｐj，Ｐk）から算出された原点軌道関数Ｃjk*（μjk）とを用いて、上記式（１１）で示した評価関数Ｆ（μij，μjk）を０もしくは最小化する（μij，μjk）を算出する。そして、算出された（μij，μjk）を上記式（９）および式（１０）に代入することで移動体原点位置を得ることができ、また、上記式（１２．１），式（１２．２）に代入することで移動体Ｍの姿勢である方位角を得ることができる。
【００９８】
また、評価関数Ｆ（μij，μjk）を最小化する（μij，μjk）を算出するために、最急降下法などの探索的な数値解法を用いてもよい。この場合、探索初期値となる値をどうとるかによって最適解が得られるか、また繰り返し探索回数が大きく異なる。そのため探索初期値としてＧＰＳ９１３および電子コンパス９１４から取得した位置および方位角から導出される値を用いてもよい。また、位置計測を連続的に実行している場合には、前回計測済みの位置から求めた位置および方位角から導出される値や、前回の（μij，μjk）の値そのものを用いて探索開始してもよい。
【００９９】
（移動体標定処理手順）
つぎに、この発明の実施の形態にかかる移動体標定処理手順について説明する。図１３は、この発明の実施の形態にかかる移動体標定処理手順を示すフローチャートである。図１３において、ＧＰＳ９１３により移動体Ｍの初期位置を取得するとともに、電子コンパス９１４により移動体Ｍの方位角を取得する（ステップＳ１３０１）。
【０１００】
そして、画像処理部１１０１により、カメラＣＭｉにより撮影された撮影画像の中からマーカー画像を抽出する（ステップＳ１３０２）。そして、検出部１１０２により、水平画角αiを算出して（ステップＳ１３０３）、世界座標系３０３における各マーカーＰi
*，Ｐj*，Ｐk*のマーカーデータＤi，Ｄj，Ｄkを検出する（ステップＳ１３０４
）。
【０１０１】
そして、特定部１１０３により、まず、上記式（３）を用いてマーカーＰi*のマーカーラインＬiとマーカーＰj*のマーカーラインＬjを算出し（ステップＳ１３０５）、仮想光学中心算出部１１３１により、上記式（４．１）〜（４．５）を用いてマーカーラインＬi，Ｌjどうしの交点となる仮想光学中心Ｑijを算出する（ステップＳ１３０６）。
【０１０２】
つぎに、マーカー間角度算出部１１３２ｉｊにより、仮想光学中心算出部１１３１によって算出された仮想光学中心Ｑijから見た２つのマーカーＰi，Ｐjのマーカー間角度θij*を、上記式（５）を用いて算出する（ステップＳ１３０７）。
【０１０３】
また、マーカー円算出部１１３３ｉｊにより、マーカー間角度算出部１１３２ｉｊによって算出されたマーカー間角度θij*から推定される仮想光学中心Ｑijの世界座標系３０３における位置を特定するマーカー円Ｅijの中心位置Ｏij*，半径ｒij*を、上記式（７．１），式（７．２）を用いて算出する（ステップＳ１３０８）。
【０１０４】
また、関数決定部１１３４ｉｊにより、仮想光学中心Ｑijから見た一方のマーカーＰiと移動体原点Ｏmとの間のマーカー角度φimを、上記式（６．１）〜式（６．３）を用いて算出するとともに、仮想光学中心Ｑijから見た他方のマーカーＰjと移動体原点Ｏmとの間のマーカー角度φjmを、下記式（６．２），式（６．４），式（６．５）を用いて算出する（ステップＳ１３０９）。
【０１０５】
なお、上述したステップＳ１３０５〜ステップＳ１３０９までの処理は、マーカー間角度算出部１１３２ｊｋ，マーカー円算出部１１３３ｊｋ，関数決定部１１３４ｊｋを用いて、マーカーＰj*，Ｐk*のマーカーデータＤj，Ｄkについても同様に実行する。
【０１０６】
このあと、関数決定部１１３４により、変数決定処理を実行する（ステップＳ１３１０）。すなわち、上記式（８）および式（９）を用いて、マーカー対（Ｐi，Ｐj），（Ｐj，Ｐk），・・・ごとに、原点軌道関数Ｃjk*（μjk），Ｃjk*（μjk），・・・を決定する。そして、この原点軌道関数Ｃjk*（μjk），Ｃjk*（μjk）を上記式（１１）に代入することで、評価関数Ｆ（μij，μjk）を０もしくは最小化する変数（μij，μjk）を決定する。
【０１０７】
そして、標定部１１０４により、算出された（μij，μjk）を上記式（９）および式（１０）に代入することで、世界座標系３０３における移動体原点位置Ｃij*を算出する（ステップＳ１３１１）。また、上記式（１２．１），式（１２．２）に代入することで移動体Ｍの姿勢である方位角ω（μij）を算出する（ステップＳ１３１２）。
【０１０８】
このあと、移動体標定を継続するか否かを判断する（ステップＳ１３１３）。移動体標定を継続する場合（ステップＳ１３１３：Ｙｅｓ）、ステップＳ１３０２に戻る。この場合、ステップＳ１３１１，ステップＳ１３１２で算出された移動体原点位置Ｃij*および方位角ω（μij）を用いて、あらたに移動体原点位置および方位角を算出することとなる。一方、移動体標定を継続しない場合（ステップＳ１３１３：Ｎｏ）、一連の移動体標定処理を終了する。
【０１０９】
つぎに、上述した変数決定処理（ステップＳ１３１０）の具体的な処理手順について説明する。図１４は、変数決定処理（ステップＳ１３１０）の具体的な処理手順を示すフローチャートである。
【０１１０】
まず、変数（μij，μjk）の初期値を注目点（λij，λjk）とし、そのステップ幅をΔλとする（ステップＳ１４０１）。つぎに、注目点（λij，λjk）を評価関数Ｆに代入するとともに、注目点（λij，λjk）の近傍の値（ここでは、周囲４近傍）を評価関数Ｆに代入する（ステップＳ１４０２）。
【０１１１】
ここで、注目点（λij，λjk）の周囲４近傍について説明する。図１５は、注目点（λij，λjk）の周囲４近傍を示す説明図である。図１５において、符号１５００で示される注目点（λij，λjk）の周囲４近傍（符号１５０１〜１５０４で表記）は、図８に示した評価関数Ｆの座標系にしたがって、ステップ幅Δλ増減した値となる。
【０１１２】
そして、図１４において、周囲４近傍の評価関数Ｆの値がいずれも注目点（λij，λjk）の評価関数Ｆの値より大きいか否かを判断する（ステップＳ１４０３）。周囲４近傍の評価関数Ｆの値のうちいずれか一つが注目点（λij，λjk）の評価関数Ｆの値以下である場合（ステップＳ１４０３：Ｎｏ）、４近傍の評価関数Ｆの値のうち最小の値をとる座標値（符号１５０１〜１５０４のいずれか）をあらたな注目点（λij，λjk）とし（ステップＳ１４０４）、ステップＳ１４０２に戻る。
【０１１３】
一方、周囲４近傍の評価関数Ｆの値がいずれも注目点（λij，λjk）の評価関数Ｆの値より大きい場合（ステップＳ１４０３：Ｙｅｓ）、ステップ幅Δλがしきい値λｔ（たとえば、λｔ＝π／１０００）より小さいか否かを判断する（ステップＳ１４０５）。
【０１１４】
ステップ幅Δλがしきい値λｔ以上である場合（ステップＳ１４０５：Ｎｏ）、ステップ幅Δλを半分にして（ステップＳ１４０６）、ステップＳ１４０２に戻る。一方、ステップ幅Δλがしきい値λｔより小さい場合（ステップＳ１４０５：Ｙｅｓ）、最終的に得られた注目点（λij，λjk）を変数（μij，μjk）の値に決定する（ステップＳ１４０７）。
【０１１５】
これにより、十分な精度の解が求まったものとして処理を終了する。この変数決定処理は、μの角度空間での数値解法であるが、世界座標系３０３と同じ直交座標空間でおこなっても結果として評価関数Ｆの最小化をおこなうのであれば同様である。
【０１１６】
以上説明したように、移動体標定装置、移動体標定方法、および移動体標定プログラムによれば、移動体へのカメラの設置自由度が高く、かつ、移動体標定の高精度化を図ることができるという効果を奏する。
【０１１７】
なお、本実施の形態で説明した移動体標定方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。このプログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。またこのプログラムは、インターネット等のネットワークを介して配布することが可能な伝送媒体であってもよい。
【０１１８】
（付記１）移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をコンピュータに実行させる移動体標定プログラムであって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出させる検出工程と、
前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定させるとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定させる特定工程と、
前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定させる標定工程と、
を前記コンピュータに実行させる移動体標定プログラム。
【０１１９】
（付記２）前記第１および第２のマーカーを望む第１の仮想光学中心と、前記第２および第３のマーカーを望む第２の仮想光学中心とを算出させる仮想光学中心算出工程と、
前記仮想光学中心算出工程によって算出された第１の仮想光学中心からみた前記第１および第２のマーカーの間の第１のマーカー間角度と、前記仮想光学中心算出工程によって算出された第２の仮想光学中心からみた前記第２および第３のマーカーの間の第２のマーカー間角度とを算出させるマーカー間角度算出工程と、を前記コンピュータに実行させ、
前記特定工程は、
前記マーカー間角度算出工程によって算出された第１のマーカー間角度と、前記第１の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第１の仮想光学中心からの前記第２のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第１の移動体原点軌道を特定させるとともに、
前記マーカー間角度算出工程によって算出された第２のマーカー間角度と、前記第２の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第２の仮想光学中心からの前記第３のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第２の移動体原点軌道を特定させることを特徴とする付記１に記載の移動体標定プログラム。
【０１２０】
（付記３）前記第１の移動体原点軌道を表現する関数が、第１の変数を用いて定義され、かつ、前記第２の移動体原点軌道を表現する関数が、第２の変数を用いて定義されている場合、前記第１の移動体原点軌道を表現する関数の値と第２の移動体原点軌道を表現する関数の値との差分を数値解析で評価する評価関数が最小となるように、前記第１および第２の変数の値を決定させる変数決定工程を前記コンピュータに実行させ、
前記標定工程は、
前記変数決定工程によって決定された前記第１の変数の値を前記第１の移動体原点軌道関数に与え、かつ、前記変数決定工程によって決定された前記第２の変数の値を前記第２の移動体原点軌道関数に与えることにより、前記未知の移動体原点位置を、前記第１および第２の移動体原点軌道の交点座標に標定させることを特徴とする付記２に記載の移動体標定プログラム。
【０１２１】
（付記４）前記標定工程は、
前記第１および第２のマーカーと前記第１の仮想光学中心とが同一円周上に位置する第１のマーカー円を表現する関数が前記第１の変数を用いて定義され、前記第２および第３のマーカーと前記第２の仮想光学中心とが同一円周上に位置する第２のマーカー円を表現する関数が前記第２の変数を用いて定義されている場合、前記変数決定工程によって決定された前記第１の変数の値を前記第１のマーカー円を表現する関数に与え、かつ、前記変数決定工程によって決定された前記第２の変数の値を前記第２のマーカー円を表現する関数に与えることにより、前記移動体の方位角を標定させることを特徴とする付記３に記載の移動体標定プログラム。
【０１２２】
（付記５）前記第１〜第３のマーカーの位置および方向が、前記移動体に搭載されているＧＰＳおよび電子コンパスによって取得された前記移動体の位置および方位を用いて検出された場合、前記標定工程によって標定された移動体原点位置および前記移動体の方位角を用いて、前記検出工程、前記特定工程、および前記標定工程を再度、前記コンピュータに実行させることを特徴とする付記４に記載の移動体標定プログラム。
【０１２３】
（付記６）付記１〜５のいずれか一つに記載の移動体標定プログラムを記録した前記コンピュータに読み取り可能な記録媒体。
【０１２４】
（付記７）移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定装置であって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出手段と、
前記検出手段によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出手段によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定手段と、
前記未知の移動体原点位置を、前記特定手段によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定手段と、
を備えることを特徴とする移動体標定装置。
【０１２５】
（付記８）移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定方法であって、
前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出工程と、
前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定工程と、
前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定工程と、
を含んだことを特徴とする移動体標定方法。
【産業上の利用可能性】
【０１２６】
以上のように、本発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法は、移動体標定、とりわけ自動車の高精度測位などに用いられ、ＩＴＳ（Ｉｎｔｅｌｌｉｇｅｎｔ  ｔｒａｎｓｐｏｒｔａｔｉｏｎ  ｓｙｓｔｅｍｓ）分野において車両位置の高精度測位や路車協調、車車協調の安全機能を実現する上での必要不可欠な要素技術として有用である。
【図面の簡単な説明】
【０１２７】
【図１】この発明の実施の形態にかかる移動体標定移動体標定を示す説明図である。
【図２−１】この発明の実施の形態にかかる移動体原点軌道を示す説明図である。
【図２−２】１）〜３）の条件を満たす移動体原点軌道Ｔを示す説明図である。
【図３】この発明の実施の形態にかかる各種座標系を示す説明図である。
【図４】任意のカメラＣＭｉによって１つのマーカーＰiが撮影された場合のマーカー方位推定を示す説明図である。
【図５】マーカー間角度推定を示す説明図である。
【図６】マーカー間角度θij*からの移動体原点Ｏmの世界座標系３０３における推定を示す説明図である。
【図７】ＧＰＳの計測値からの変数μijの初期値の算出手法を示す説明図である。
【図８】式（１１）で示した評価関数Ｆ（μij，μjk）の解空間となる曲面を示す説明図である。
【図９】この発明の実施の形態にかかる移動体標定装置のハードウェア構成を示すブロック図である。
【図１０】マーカーＤＢの記憶内容を示す説明図である。
【図１１】この発明の実施の形態にかかる移動体標定装置の機能的構成を示すブロック図である。
【図１２】特定部１１０３の詳細な機能的構成を示すブロック図である。
【図１３】この発明の実施の形態にかかる移動体標定処理手順を示すフローチャートである。
【図１４】変数決定処理（ステップＳ１３１０）の具体的な処理手順を示すフローチャートである。
【図１５】注目点（λij，λjk）の周囲４近傍を示す説明図である。
【図１６】移動体に搭載したカメラにより撮影されたマーカーどうしが成す角度から位置標定をおこなう手法を示す説明図である。
【図１７】全方位カメラの光学系および当該光学系により撮像される画像例を示す説明図である。
【図１８】複数のカメラＣＭ１〜ＣＭ３を自動車に設置した場合の撮像例を示す説明図である。
【符号の説明】
【０１２８】
３０１  移動体座標系
３０２  カメラ座標系
３０３  世界座標系
４００  画像座標系
９１３  ＧＰＳ
９１４  電子コンパス
１１００  移動体標定装置
１１０１  画像処理部
１１０２  検出部
１１０３  特定部
１１０４  標定部
１１３１  仮想光学中心算出部
１１３２  マーカー間角度算出部
１１３３  マーカー円算出部
１１３４  関数決定部
【図１】
2008051745.tif 000017
【図２−１】
2008051745.tif 000018
【図２−２】
2008051745.tif 000019
【図３】
2008051745.tif 000020
【図４】
2008051745.tif 000021
【図５】
2008051745.tif 000022
【図６】
2008051745.tif 000023
【図７】
2008051745.tif 000024
【図８】
2008051745.tif 000025
【図９】
2008051745.tif 000026
【図１０】
2008051745.tif 000027
【図１１】
2008051745.tif 000028
【図１２】
2008051745.tif 000029
【図１３】
2008051745.tif 000030
【図１４】
2008051745.tif 000031
【図１５】
2008051745.tif 000032
【図１６】
2008051745.tif 000033
【図１７】
2008051745.tif 000034
【図１８】
2008051745.tif 000035