JP2008051745A - 移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法 - Google Patents

Abstract

【課題】移動体へのカメラの設置自由度が高く、かつ移動体標定の高精度化を図ること。
【解決手段】仮想カメラＶＣｉｊの位置Ｖ_ijと移動体原点Ｃ_ijとの相対位置関係１）〜３）より、移動体原点Ｃ_ijが存在する位置候補となる移動体原点軌道を特定する。
１）移動体原点位置Ｃ_ij−仮想カメラＶＣｉｊ間の距離ｄが一定
２）移動体座標系（移動体原点Ｏ_m）からみた仮想カメラＶＣｉｊの方向が一定（仮想カメラＶＣｉｊから見たマーカーＰ_jと移動体原点位置Ｃ_ijとの間の角度が一定）
３）仮想カメラＶＣｉｊがマーカー円Ｅ_ij上に存在（仮想カメラＶＣｉｊから見たマーカーＰ_i，Ｐ_j間の角度が一定）
移動体原点軌道Ｔが複数ある場合に各移動体原点軌道Ｔの交点を求めることで移動体Ｍの位置を特定できる。
【選択図】図２−１

Description

この発明は、移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法に関する。

移動体の位置を計測する手段としてはＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）が広く用いられている。ＧＰＳ単独測位は１０［ｍ］程度の精度があり、ドライバーへの経路提示といったナビゲーションサービスのための測位技術としては実用的ではあるが、介入制御、自動走行などの高度な車両コントロールのための測位としては精度が足りない。

また、ＧＰＳ干渉測位方式（ＲＴＫ，ＶＲＳ−ＲＴＫ等）を用いると数［ｃｍ］の測位精度を実現できるが、機器が非常に高価であること、反射波により精度が悪化すること、トンネルなどで電波が遮断された後の復帰に非常に時間がかかることなどから路上で利用するには問題が多い。

一方、移動体に搭載したカメラにより複数のマーカーを撮影し、マーカーどうしが成す角度から位置標定をおこなう手法がある。図１６は、移動体に搭載したカメラにより撮影されたマーカーどうしが成す角度から位置標定をおこなう手法を示す説明図である。

図１６において、複数のマーカーＰ₁，Ｐ₂，Ｐ₃，・・・の位置が既知である場合、観測された２つのマーカーＰ_i，Ｐ_j（ｉ，ｊ＝１，２，３，・・・、ただしｉ≠ｊ）と移動体Ｍは、下記式（１．１）であらわされる中心位置Ｏ_ijと下記式（１．２）であらわされる半径ｒ_ijとを持つ円（マーカー円Ｅ_ij）の円周上に存在する。

上記式（１．２）において、θ_ijは、２つのマーカーＰ_i，Ｐ_jの成すマーカー間角度である。θ_ijは、移動体原点Ｏ_mから撮影された撮影画像により算出される。３個以上のマーカーが検知された場合、複数のマーカー円Ｅ_ijを式（１．１）および式（１．２）により得ることができるため、これらのマーカー円Ｅ_ij（図１６ではＥ₁₂，Ｅ₂₃）の交点位置を求めることで連続的に移動体位置を得ることができる。

上記方式は精度高く移動体位置を求めることができるものの、移動体原点Ｏ_mを基準とした水平３６０度を計測範囲とするマーカー方位計測手段が必要になる。そのため、移動体原点Ｏ_mに光学中心を持つ特殊な全方位カメラを用いることが必要である。

図１７は、全方位カメラの光学系および当該光学系により撮像される画像例を示す説明図である。図１７において、投影面ｆは光学中心を通る円直線を中心軸Ｙとする円柱の側面により構成されている。撮影された画像は、水平軸を方位角θと対応して記録される。このためマーカー投影像の水平画素位置から容易にマーカー方位角とマーカー間角度（図１７ではθ₁₂，θ₂₃）を算出することができる。

このような全方位カメラを実現するために、たとえば、下記特許文献１では、光学中心を軸に３６０度回転させる機構を持たせており、また、下記特許文献２では、特殊な反射板とレンズからなる光学系を用いている。

また、上述のような全方位カメラを用いずに、複数カメラから３６０度視野のパノラマ撮影画像を作成する方式及び商品が各種存在している。図１８は、複数のカメラＣＭ１〜ＣＭ３を自動車に設置した場合の撮像例を示す説明図である。図１８において、各カメラＣＭ１〜ＣＭ３はその視界内のマーカーＰ₁，Ｐ₂，Ｐ₃，・・・を撮像することができる。これらカメラＣＭ１〜ＣＭ３を複数組み合わせることで、周辺視野を生成することができる（たとえば、下記特許文献３を参照。）。

特開平１１−８３４８０号公報 特開２０００−３３７８８７号公報 特開２００６−５４６６２号公報

しかしながら、上述した全方位カメラを用いた従来技術では、いずれも機構的に複雑であり高価かつ大型になる。また、移動体に設置する場合には全方位カメラを３６０度の視界が開けている場所に設置する必要があるが、自動車などの移動体では、一般的にはその頭頂部しか設置することができず、設置自由度はきわめて低い。特に、自動車についてはその外形デザイン上が重視されるため、極めて重大な制約を課すことになるという問題があった。

また、複数のカメラを用いた従来技術では、各カメラＣＭ１〜ＣＭ３の光学中心のズレを積極的に解決していない。たとえば、図１８に示した例において、カメラＣＭ１〜ＣＭ３で撮像されたマーカーＰ₁，Ｐ₂，Ｐ₃は、カメラＣＭ１〜ＣＭ３の独自の座標系によりマーカーＰ₁，Ｐ₂，Ｐ₃の方向を計測することが可能（図１８中、○で表示）であるが、移動体原点Ｏ_mを光学中心とした場合のマーカーＰ₁，Ｐ₂，Ｐ₃の方向を計測することは、カメラＣＭ１〜ＣＭ３の光学中心のズレにより不可能ないしは可能であったとしても精度が悪いという問題があった（図１８中、×で表示）。

この発明は、上述した従来技術による問題点を解消するため、移動体へのカメラの設置自由度が高く、かつ、移動体の位置標定の高精度化を図ることができる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法を提供することを目的とする。

上述した課題を解決し、目的を達成するため、この発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法は、移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法であって、前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出し、位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定し、前記未知の移動体原点位置を、特定された第１および第２の移動体原点軌道の交点座標に標定することを特徴とする。

また、上記発明において、前記第１および第２のマーカーを望む第１の仮想光学中心と、前記第２および第３のマーカーを望む第２の仮想光学中心とを算出し、算出された第１の仮想光学中心からみた前記第１および第２のマーカーの間の第１のマーカー間角度と、算出された第２の仮想光学中心からみた前記第２および第３のマーカーの間の第２のマーカー間角度とを算出し、算出された第１のマーカー間角度と、前記第１の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第１の仮想光学中心からの前記第２のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第１の移動体原点軌道を特定するとともに、算出された第２のマーカー間角度と、前記第２の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第２の仮想光学中心からの前記第３のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第２の移動体原点軌道を特定することとしてもよい。

また、上記発明において、前記第１の移動体原点軌道を表現する関数が、第１の変数を用いて定義され、かつ、前記第２の移動体原点軌道を表現する関数が、第２の変数を用いて定義されている場合、前記第１の移動体原点軌道を表現する関数の値と第２の移動体原点軌道を表現する関数の値との差分を数値解析で評価する評価関数が最小となるように、前記第１および第２の変数の値を決定し、決定された前記第１の変数の値を前記第１の移動体原点軌道関数に与え、かつ、決定された前記第２の変数の値を前記第２の移動体原点軌道関数に与えることにより、前記未知の移動体原点位置を、前記第１および第２の移動体原点軌道の交点座標に標定することとしてもよい。

また、上記発明において、前記第１および第２のマーカーと前記第１の仮想光学中心とが同一円周上に位置する第１のマーカー円を表現する関数が前記第１の変数を用いて定義され、前記第２および第３のマーカーと前記第２の仮想光学中心とが同一円周上に位置する第２のマーカー円を表現する関数が前記第２の変数を用いて定義されている場合、前記第１の変数の値を前記第１のマーカー円を表現する関数に与え、かつ、前記第２の変数の値を前記第２のマーカー円を表現する関数に与えることにより、前記移動体の方位角を標定することとしてもよい。

また、上記発明において、前記第１〜第３のマーカーの位置および方向が、前記移動体に搭載されているＧＰＳおよび電子コンパスによって取得された前記移動体の位置および方位を用いて検出された場合、標定された移動体原点位置および前記移動体の方位角を用いて、移動体標定をおこなうこととしてもよい。

この発明によれば、移動体標定（位置や方位角の標定）を移動体に搭載された複数のカメラを用いて実現することができる。また、すでに複数のカメラが搭載済みの移動体については、本発明を取り入れるだけで、移動体標定をおこなうことができる。また、複数のカメラが未搭載の移動体についても、全方位カメラのような設置位置の制約を受けずに複数のカメラを設置することができ、移動体標定を移動体のデザインを損なうことなく高精度に実現することができる。また、ＧＰＳや電子コンパスなどの低精度の移動体標定データを用いて本発明を繰り返し実行することで、移動体標定を高精度に算出することができる。

本発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法によれば、移動体へのカメラの設置自由度が高く、かつ、移動体標定の高精度化を図ることができるという効果を奏する。

以下に添付図面を参照して、この発明にかかる移動体標定プログラム、該プログラムを記録した記録媒体、移動体標定装置、および移動体標定方法の好適な実施の形態を詳細に説明する。本実施の形態では全方位カメラでマーカーを検知するかわりに、複数の一般的なカメラを組み合わせることでマーカーを検知する。自動車に関しては既に自動車周辺の視界確保用に前方、後側方、後方に複数台のカメラを装備しているものもあるため、これらを利用してもよい。

（移動体標定の原理）
まず、この発明の実施の形態にかかる移動体標定の原理について説明する。図１は、この発明の実施の形態にかかる移動体標定の原理を示す説明図である。この移動体標定では、移動体Ｍに搭載された異なるカメラＣＭｉ，ＣＭｊ（ｉ，ｊ＝１，２，３，・・・、ただしｉ≠ｊ）で撮影される２つのマーカーＰ_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間の角度が一定）

図２−２は、上記１）〜３）の条件を満たす移動体原点軌道Ｔを示す説明図である。移動体原点軌道Ｔが複数ある場合（たとえば、３点以上のマーカーＰ₁，Ｐ₂，Ｐ₃，・・・が観測された場合）に各移動体原点軌道Ｔの交点を求めることで移動体Ｍの位置を特定できる。各移動体原点軌道Ｔの交点は逐次近似法などの数値解法で求めることができる。

（移動体標定手法）
つぎに、上述した移動体標定の原理を用いた移動体標定の手法について説明する。図３は、この発明の実施の形態にかかる各種座標系を示す説明図である。図３において、移動体座標系３０１は、移動体Ｍの進行方向を示すＹ_m軸と、Ｙ_m軸と直交するＸ_m軸とで水平面をあらわす座標系である。

カメラ座標系３０２は、カメラ原点Ｏ_cを各カメラＣＭｉの光学中心とし、カメラＣＭｉの光軸方向を示すＹ_ci軸と、Ｙ_ci軸と直交するＸ_ci軸とで水平面をあらわす座標系である。カメラ座標系３０２は、カメラＣＭｉの個数分存在する。また、世界座標系３０３は、所定の基準点を原点Ｏ_wとし、南北方向を示すＹ_w軸と、東西方向を示すＸ_w軸とで水平面をあらわす座標系である。各座標系の鉛直方向を示すＺ軸（不図示）は図３の紙面を突き刺す方向である。各座標系３０１〜３０３間の縮尺変化はなくＺ軸まわりの回転と並行移動で座標系変換が可能とする。なお、図３中、Ｔ_cm，Ｔ_mwは座標間移動距離である。

図４は、任意のカメラＣＭｉによって１つのマーカーＰ_iが撮影された場合のマーカー方位推定を示す説明図である。マーカーＰ_iはカメラＣＭｉが撮影した画像座標系４００（Ｘｐ−Ｙｐ）中で（ｘ_p，ｙ_p）の位置に撮像されたとすると、カメラ座標系３０２上では光軸方向（Ｙ_c軸）から角度α回転した方向にマーカーＰ_iが存在する。このときαは下記式（２）であらわされる。

ここで、ここでΘ_HはカメラＣＭｉの水平画角でありＷ_pは画像の幅を示している。カメラ座標系３０２から移動体座標系３０１への変換が（ｔ_x，ｔ_y）の水平移動と角度βの回転移動であるとき、移動体座標系３０１におけるカメラＣＭｉのカメラ原点（光学中心）Ｏ_ciとマーカーＰ_iを通る直線（マーカーライン）Ｌ_iは下記式（３）であらわされる。

同様に他のカメラとマーカーについてもそのカメラ原点（光学中心）とマーカーを通る直線（マーカーライン）が上記式（３）により算出されることとなる。

図５は、マーカー間角度推定を示す説明図である。図５では、複数のカメラ（ＣＭｉ，ＣＭｊ）から得られたマーカーラインＬ_i，Ｌ_jを用いてマーカー間角度θ_ijを推定する。具体的には、各マーカーラインＬ_i，Ｌ_jどうしの交点を求め、その交点を基準としたマーカーＰ_i，Ｐ_jの成す角度（マーカー間角度θ_ij）を算出する。任意の２マーカーＰ_i，Ｐ_jを同一のカメラＣＭｉで撮影した場合はマーカーラインＬ_i，Ｌ_jの交点はそのカメラＣＭｉのカメラ原点（光学中心）Ｏ_ciと一致する。

一方、異なるカメラＣＭｉ，ＣＭｊで撮影されている場合は、点Ｑ_ijが交点となる。この交点Ｑ_ijを「仮想光学中心Ｑ_ij」と称す。仮想光学中心Ｑ_ijは、下記式（４．１）〜式（４．５）であらわされる。

また、仮想光学中心Ｑ_ijから見た２つのマーカーＰ_i，Ｐ_jのマーカー間角度θ_ij ^*は、下記式（５）であらわされる。

また、仮想光学中心Ｑ_ijから見た一方のマーカーＰ_iと移動体原点Ｏ_mと間の角度φ_imは、下記式（６．１）〜式（６．３）であらわされ、仮想光学中心Ｑ_ijから見た他方のマーカーＰ_jと移動体原点Ｏ_mと間の角度φ_jmは、下記式（６．２），式（６．４），式（６．５）であらわされる。

なお、図５中、カメラ原点Ｏ_cjはカメラＣＭｊのカメラ原点（光学中心）であり、Ｙ_ci軸は、カメラＣＭｊの光軸方向を示すカメラ座標系の座標軸であり、Ｘ_cj軸は、Ｙ_cj軸と直交することで水平面をあらわすカメラＣＭｊのカメラ座標系の座標軸である。

図６は、マーカー間角度θ_ij ^*からの移動体原点Ｏ_mの世界座標系３０３における推定を示す説明図である。以降、各記号において、「^*」が付されている記号は、世界座標系３０３における値（座標位置または角度）を示している。たとえば、マーカーＰ_i ^*は、マーカーＰ_iの世界座標系３０３における座標位置を示している。図６において、マーカー間角度θ_ij ^*から推定される仮想光学中心Ｑ_ijの世界座標系３０３における位置は、中心位置Ｏ_ij ^*，半径ｒ_ij ^*の円周上の点Ｑ_ij ^*である。中心位置Ｏ_ij ^*，半径ｒ_ij ^*は、下記式（７．１），式（７．２）であらわされる。

ただし、Ｐ_i ^*＝（ｐ_ix ^*，ｐ_iy ^*），Ｐ_j ^*＝（ｐ_jx ^*，ｐ_jy ^*）は世界座標系３
０３におけるマーカー位置座標である。

移動体原点Ｏ_mと仮想光学中心Ｑ_ij ^*間の距離｜Ｑ_ij｜と、移動体原点Ｏ_mとマーカーＰ_iとの成す角度φ_imは、移動体座標系３０１でも世界座標系３０３でも共通であるため、移動体Ｍの位置Ｃ_ij ^*は下記式（８）を満たす。

ここで、移動体Ｍの位置Ｃ_ij ^*は、上記式（７．１）〜（７．２）で示される円周上の１点であるとしかわかっておらず、式（８）だけでは移動体Ｍの位置は特定できない。仮に仮想光学中心Ｑ_ij ^*を変数μ_ijを用いて極座標系であらわすと下記式（９）のようになる。

移動体Ｍの位置Ｃ_ij ^*は変数μ_ijの関数として下記式（１０）であらわされる。

式（１０）では、任意の一対のマーカーＰ_i，Ｐ_jに対して計算することができる。たとえば同様に一対のマーカーＰ_j，Ｐ_kに対しても式（１０）を計算すると、その位置Ｃ_jk ^*はＣ_ij ^*と同じ位置を指し示しているから、下記式（１１）のＦ（μ_ij，μ_jk）の値を０とする変数μ_ijおよびμ_jkを求め、この変数μ_ijおよびμ_jkを式（１０）に代入することで世界座標系３０３における移動体位置Ｃ_ij ^*を特定することができる。

また、移動体Ｍの姿勢を移動体座標系３０１のＹ_m軸方向の指し示す方位角ω（μ_ij）は、下記式（１２．１），式（１２．２）で特定することができる。

なお、変数μ_ijおよびμ_jkを求めるには解析的におこなってもよいし数値解法を用いてもよい。数値解法を用いる場合、結果は初期値に依存するが、移動体位置の初期値として連続的な計測の場合は前回の位置を用いたり、他の移動体標定手段から取得した値を用いることで良好な初期値を得ることができる。後者の場合、他の移動体標定手段の精度は低くても良く、たとえば、従来のＧＰＳや電子コンパスによる移動体標定などを利用できる。

ここで、上述したＧＰＳの計測値からの変数μ_ijの初期値の算出手法について説明する。図７は、ＧＰＳの計測値からの変数μ_ijの初期値の算出手法を示す説明図である。図７において、ＧＰＳにより計測された緯度経度を世界座標系３０３（直交座標系）に変換した位置をＧ_ij ^*＝（ｇ_ijx，ｇ_ijy）とし、仮想光学中心座標をＲ_ij ^*とする。

このときＲ_ij ^*の座標値は未知であるが、その座標値は上記式（７．１），（７．２）で求められた中心位置Ｏ_ij ^*，半径ｒ_ij ^*であらわされる円Ｅ_ijの円周上にあることがわかっている。一方、マーカーＰ_i ^*と仮想光学中心座標をＲ_ij ^*と緯度経度座標Ｇ_ij ^*とが成す角度が角度φ_im（上記式（６．１）〜式（６．３）を参照）に近似できるとすると、仮想光学中心座標Ｒ_ij ^*は、下記式（１３．１），式（１３．２）であらわされる円Ｅの円周上に存在する。

したがって、上記円Ｅと円Ｅ_ijとの交点を求めることで仮想光学中心座標Ｒ_ij ^*が求まる。また、仮想光学中心座標Ｒ_ij ^*は下記式（１４）であらわされる。

上記式（１４）において、変数λ_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）の周囲４近傍を示す説明図である。 移動体に搭載したカメラにより撮影されたマーカーどうしが成す角度から位置標定をおこなう手法を示す説明図である。 全方位カメラの光学系および当該光学系により撮像される画像例を示す説明図である。 複数のカメラＣＭ１〜ＣＭ３を自動車に設置した場合の撮像例を示す説明図である。

符号の説明

３０１ 移動体座標系
３０２ カメラ座標系
３０３ 世界座標系
４００ 画像座標系
９１３ ＧＰＳ
９１４ 電子コンパス
１１００ 移動体標定装置
１１０１ 画像処理部
１１０２ 検出部
１１０３ 特定部
１１０４ 標定部
１１３１ 仮想光学中心算出部
１１３２ マーカー間角度算出部
１１３３ マーカー円算出部
１１３４ 関数決定部

Claims (5)

1. 移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をコンピュータに実行させる移動体標定プログラムであって、
   前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出させる検出工程と、
   前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定させるとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定させる特定工程と、
   前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定させる標定工程と、
   を前記コンピュータに実行させる移動体標定プログラム。
2. 前記第１および第２のマーカーを望む第１の仮想光学中心と、前記第２および第３のマーカーを望む第２の仮想光学中心とを算出させる仮想光学中心算出工程と、
   前記仮想光学中心算出工程によって算出された第１の仮想光学中心からみた前記第１および第２のマーカーの間の第１のマーカー間角度と、前記仮想光学中心算出工程によって算出された第２の仮想光学中心からみた前記第２および第３のマーカーの間の第２のマーカー間角度とを算出させるマーカー間角度算出工程と、を前記コンピュータに実行させ、
   前記特定工程は、
   前記マーカー間角度算出工程によって算出された第１のマーカー間角度と、前記第１の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第１の仮想光学中心からの前記第２のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第１の移動体原点軌道を特定させるとともに、
   前記マーカー間角度算出工程によって算出された第２のマーカー間角度と、前記第２の仮想光学中心と前記未知の移動体原点位置との間の距離と、前記第２の仮想光学中心からの前記第３のマーカーと前記未知の移動体原点位置との間の角度とが一定となるように、前記第２の移動体原点軌道を特定させることを特徴とする請求項１に記載の移動体標定プログラム。
3. 請求項１または２に記載の移動体標定プログラムを記録した前記コンピュータに読み取り可能な記録媒体。
4. 移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定装置であって、
   前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出手段と、
   前記検出手段によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出手段によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定手段と、
   前記未知の移動体原点位置を、前記特定手段によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定手段と、
   を備えることを特徴とする移動体標定装置。
5. 移動体に搭載された複数のカメラによって撮影された移動体外の各撮影画像を用いて前記移動体の標定をおこなう移動体標定方法であって、
   前記移動体外の各撮影画像にそれぞれ含まれている第１〜第３のマーカーの位置および方向を検出する検出工程と、
   前記検出工程によって位置および方向が検出された第１および第２のマーカーに基づいて、未知の移動体原点位置に関する第１の移動体原点軌道を特定するとともに、前記検出工程によって位置および方向が検出された第２および第３のマーカーに基づいて、前記未知の移動体原点位置に関する第２の移動体原点軌道を特定する特定工程と、
   前記未知の移動体原点位置を、前記特定工程によって特定された第１および第２の移動体原点軌道の交点座標に標定する標定工程と、
   を含んだことを特徴とする移動体標定方法。

Images