JP6707378B2 - 自己位置推定装置および自己位置推定方法 - Google Patents

Description

本発明は、自己位置推定装置および自己位置推定方法に関する。

現在、移動体の位置を知るための手段として、３つの基地局から通知された位置の情報と測定した基地局間の位相差とから、移動局と各基地局間の距離の差を計算し、移動体の位置を計測することが提案されている（例えば、特許文献１参照）。

また、近年、ＩＴＳ（Ｉｎｔｅｌｌｉｇｅｎｔ Ｔｒａｎｓｐｏｒｔ Ｓｙｓｔｅｍｓ：高度道路交通システム）では、移動体への情報の送信手段として電波ビーコン、光ビーコン、およびＦＭ多重放送が使用されている。移動体である車両が、光ビーコンを受信する場合、道路の環境等に応じて受信できる送信信号の数が変動する場合がある。

特許第３５９０７４４号公報

しかしながら、特許文献１に記載の技術では、光通信において受光できる送信信号の数が変動した場合に自己位置を求めることができない。

本発明は、上記の問題点に鑑みてなされたものであって、光通信において自己位置算出に利用できる送信信号の数が変動した場合であっても、自己位置を適度に求めることができる自己位置推定装置および自己位置推定方法を提供することを目的とする。

（１）上記目的を達成するため、本発明の一態様に係る自己位置推定装置は、送信装置からの発光信号を画素によって受光する受光部と、前記受光部が取得した前記発光信号のうち自己位置の算出に用いる数に応じて、前記自己位置を推定する複数のアルゴリズムの中から少なくとも１つを選択し、選択した前記アルゴリズムを用いて前記自己位置を算出する位置算出部と、を備え、前記位置算出部は、前記受光部が４つ以上の前記発光信号を取得した場合、４つ以上の前記発光信号の中から３つ以下の前記発光信号を選択し、選択した前記発光信号を用いて前記複数のアルゴリズムの中から２つ以上の前記アルゴリズムを選択し、予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムの中から少なくとも１つの前記アルゴリズムを選択、または予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムによって算出される前記自己位置の中から少なくとも１つを選択する処理、算出した前記自己位置の平均値を算出する処理、算出した前記自己位置に対して予め定められている重み付けした後に平均を算出する処理、算出した前記自己位置に対してクラスタリングを行い、クラスタに基づいて前記自己位置を算出する処理のうち少なくとも１つの処理によって前記自己位置を算出する。

（２）上記目的を達成するため、本発明の一態様に係る自己位置推定装置は、送信装置からの発光信号を画素によって受光する受光部と、前記受光部が取得した前記発光信号のうち自己位置の算出に用いる数に応じて、前記自己位置を推定する複数のアルゴリズムの中から少なくとも１つを選択し、選択した前記アルゴリズムを用いて前記自己位置を算出する位置算出部と、を備え、前記位置算出部は、前記発光信号のうち自己位置算出に利用できる数が３つの場合に、３つの前記送信装置の位置と、自己位置推定装置の姿勢とが既知のとき第１アルゴリズムを選択し、３つの前記送信装置の位置と、前記自己位置推定装置の姿勢に基づいて前記自己位置を推定し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれが既知のとき第２アルゴリズムを選択し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれに基づいて前記自己位置を推定し、３つの前記送信装置の位置のみが既知のとき第３アルゴリズムを選択し、３つの前記送信装置の位置に基づいて前記自己位置を推定する。

（３）上記目的を達成するため、本発明の一態様に係る自己位置推定方法は、受光部が、送信装置からの発光信号を画素によって受光する受光手順と、位置算出部が、前記受光部が４つ以上の前記発光信号を取得した場合、４つ以上の前記発光信号の中から３つ以下の前記発光信号を選択し、選択した前記発光信号を用いて複数のアルゴリズムの中から２つ以上の前記アルゴリズムを選択し、予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムの中から少なくとも１つの前記アルゴリズムを選択、または予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムによって算出される自己位置の中から少なくとも１つを選択する処理、算出した前記自己位置の平均値を算出する処理、算出した前記自己位置に対して予め定められている重み付けした後に平均を算出する処理、算出した前記自己位置に対してクラスタリングを行い、クラスタに基づいて前記自己位置を算出する処理のうち少なくとも１つの処理によって前記自己位置を算出する手順と、を含む。
（４）上記目的を達成するため、本発明の一態様に係る自己位置推定方法は、受光部が、送信装置からの発光信号を画素によって受光する受光手順と、位置算出部が、前記発光信号のうち自己位置算出に利用できる数が３つの場合に、３つの前記送信装置の位置と、自己位置推定装置の姿勢とが既知のとき第１アルゴリズムを選択し、３つの前記送信装置の位置と、前記自己位置推定装置の姿勢に基づいて自己位置を推定し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれが既知のとき第２アルゴリズムを選択し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれに基づいて前記自己位置を推定し、３つの前記送信装置の位置のみが既知のとき第３アルゴリズムを選択し、３つの前記送信装置の位置に基づいて前記自己位置を算出する手順と、を含む。

上述した（１）または（３）の構成によれば、光通信において自己位置算出に利用できる送信信号の数が変動した場合であっても、自己位置を適度に求めることができる。

また、上述した（１）または（３）の構成によれば、４つ以上の送信信号を自己位置算出に利用できる場合であっても、４つ以上の送信信号から３つ以下の送信信号を選択して、選択した送信信号に基づくアルゴリズムを選択することができる。これにより、上述した（１）または（３）によれば、４つ以上の送信信号が受信できた場合であっても、適度な精度にて自己位置を推定することができる。

また、上述した（２）または（４）の構成によれば、自己位置算出に利用できる送信信号の数が３つの場合、受信できた３つの送信信号に基づいて、高精度に自己位置を推定することができる。

本実施形態に係る通信システムの構成を表すシステム構成図である。 本実施形態に係る通信システムの処理のフローチャートである。 本実施形態に係るアルゴリズムの選択のフローチャートである。 本実施形態に係るアルゴリズムの選択のフローチャートである。 本実施形態に係る送信装置と自己位置推定装置を搭載する車両との間の相互通信を示す図である。 本実施形態に係る第１アルゴリズムの説明に用いる図である。 本実施形態に係る第２アルゴリズムの説明に用いる図である。 本実施形態に係る第３アルゴリズムの説明に用いる図である。 本実施形態に係る第４アルゴリズムの説明に用いる図である。 本実施形態に係る第５アルゴリズムの説明に用いる図である。 本実施形態に係る第６アルゴリズムの説明に用いる図である。 本実施形態に係る第７アルゴリズムの説明に用いる図である。 本実施形態に係る第８アルゴリズムの説明に用いる図である。 本実施形態に係る第９アルゴリズムの説明に用いる図である。 本実施形態に係る第１０アルゴリズムの説明に用いる図である。

以下、図面を参照しながら本発明の実施形態について説明する。
図１は、本実施形態に係る通信システム１の構成を表すシステム構成図である。図１に示すように、通信システム１は、自己位置推定装置１０および第１の送信装置２０−１、第２の送信装置２０−２、第３の送信装置２０−３、・・・を備えている。以下の例では、車両が自己位置推定装置１０を搭載しているとして説明する。また、以下の説明では、第１の送信装置２０−１、第２の送信装置２０−２、第３の送信装置２０−３、・・・のうちの１つを特定しない場合、単に送信装置２０という。

図１に示すように、自己位置推定装置１０は、レンズ１０１、受光部１０２、ＧＮＳＳ１０３、発振器１０４、復調器１０５、復号器１０６、制御部１０７、符号器１０８、変調器１０９、投光器１１０、記憶部１１１、座標ＤＢ１１２、第１座標探索部１１３、センサー部１１４、姿勢推定部１１５、相対変位推定部１１６、撮像部１１７、車線位置計測部１１８、座標ＤＢ１１９、第２座標探索部１２０、位置算出部１２１を備える。また、センサー部１１４は、加速度センサー１３１、レートセンサー１３２、および距離計１３３を備える。なお、自己位置推定装置１０のレンズ１０１および受光部１０２それぞれは、車両の進行方向に対して前後に搭載されている。

また、送信装置２０は、レンズ２０１、受光部２０２、ＧＮＳＳ２０３、発振器２０４、復調器２０５、復号器２０６、制御部２０７、符号器２０８、変調器２０９、投光器２１０、および記憶部２１１を備える。

送信装置２０は、例えば、信号機、ガードレール、陸橋、歩道橋、他の車両に取り付けられている。送信装置２０は、自装置を識別する識別子（ＩＤ）とタイムコードと同期クラスと情報を含む信号を符号化し、符号化した送信信号である光ビーコンを予め定められているときに互いに送信する。なお、同期クラスとは、自己位置推定装置１０の発振器１０４または送信装置２０の発振器２０４の周波数精度を示す情報である。本実施形態では、例えば、ＧＮＳＳ１０３またはＧＮＳＳ２０３に同期しているレベルの周波数精度を０、１０^−１０以下の周波数精度を１、１０^−１０より大きく１０^−９以下の周波数精度を２、１０^−６以上の周波数精度を３とする。

自己位置推定装置１０は、送信装置２０が送信した送信信号を光通信によって受信する。自己位置推定装置１０は、受信した送信信号から情報を抽出する。自己位置推定装置１０は、受信した送信信号に基づいて、送信装置２０と自己位置推定装置１０を搭載する車両との間の距離を算出する。

まず、送信装置２０について説明する。
レンズ２０１は、自己位置推定装置１０が発光した送信信号である位相変調光および環境光を含む光束を通過し、通過した光束を受光部２０２に結像する。なお、送信装置２０は、レンズ２０１と受光部２０２との間に、光学的なフィルタを備えるようにしてもよい。
受光部２０２は、複数の画素が二次元に配列された構造を有する。受光部２０２は、画素によって受光した光に応じた電荷を発生させて蓄積し、制御部２０７の制御に応じて所定のタイミングで蓄積した電荷を受信信号として復調器２０５へ出力する。

ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ（ｓ）；全地球航法衛星システムまたは汎地球航法衛星システム）２０３は、衛星（不図示）を用いた測位システムである。ＧＮＳＳ２０３は、衛星から受信した信号から基準信号を抽出し、抽出した基準信号を発振器２０４に出力する。
発振器２０４は、ＧＮＳＳ２０３から入力された基準信号に応じて、生成した同期信号を補正し、補正した同期信号を変調器２０９に出力する。または、発振器２０４は、ＧＮＳＳ２０３が出力した基準信号を用いて同期信号を生成し、生成した同期信号を変調器２０９に出力する。なお、発振器２０４は、ＧＮＳＳ２０３の代わりに他の送信装置２０から同期信号を有線や無線にて受け取って発振するようにしてもよい。

復調器２０５は、受光部２０２が出力する受信信号に対して、変調器２０９が用いる変調方式に応じた復調を行い、復調した受信信号を復号器２０６に出力する。変調器２０９で行う変調方式は、例えば、２π／３−ＤＢＰＳＫ（Ｄｉｆｆｅｒｅｎｔｉａｌ Ｂｉｎａｒｙ Ｐｈａｓｅ Ｓｈｉｆｔ Ｋｅｙｉｎｇ）等の方式である。なお、画素の制御は、復調器２０５が行うようにしてもよい。
復号器２０６は、復調器２０５が復調した受信信号を、符号器２０８が用いる符号化に応じて復号し、復号した受信信号を制御部２０７に出力する。

記憶部２１１は、送信装置２０のＩＤと、変調や符号化に用いる情報等を記憶する。
制御部２０７は、受光部２０２が有する画素を制御する。また、制御部２０７は、投光器２１０を制御する。制御部２０７は、記憶部２１１が記憶する情報を用いて送信情報を生成し、生成した送信情報を符号器２０８に出力する。制御部２０７は、復号器２０６が出力する自己位置推定装置１０からの送信情報を受信し、受信した送信情報から、ＩＤ、タイムコード、同期クラス、および搬送波の位相情報を抽出する。制御部２０７は、復号器２０６が出力する自己位置推定装置１０のＩＤと遅れ時間を取得する。制御部２０７は、自己位置推定装置１０のＩＤとタイムコードと同期クラスと遅れ時間を符号器２０８へ出力する。

符号器２０８は、制御部２０７が出力する送信情報を符号化してビット列を生成する。符号器２０８は、生成したビット列を変調器２０９に出力する。
変調器２０９は、符号器２０８が出力するビット列を、例えばＤＢＰＳＫ方式に従って、発振器２０４が出力する同期信号を用いて変調して搬送波を生成する。変調器２０９は、生成した搬送波を投光器２１０に出力する。なお、変調器２０９で行う変調方式は、例えば、２π／３−ＤＢＰＳＫ（Ｄｉｆｆｅｒｅｎｔｉａｌ Ｂｉｎａｒｙ Ｐｈａｓｅ Ｓｈｉｆｔ Ｋｅｙｉｎｇ）等の方式である。なお、変調器２０９が行う変調方式は、２π／３−ＤＢＰＳＫ変調方式に限られない。

投光器２１０は、変調器２０９が生成した搬送波に基づいて位相変調し、位相変調した送信信号である位相変調光を制御部２０７の制御に応じて自己位置推定装置１０へ送信する。投光器２１０は、例えば高レート（繰り返し周波数）の例えば可視光パルスを送信することのできる発光ダイオード（ＬＥＤ：Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）またはレーザダイオードを備える。また、投光器２１０は、高レートの赤外線パルスを送信する発光ダイオードまたはレーザダイオードを備えていてもよい。

次に、自己位置推定装置１０について説明する。
レンズ１０１は、送信装置２０が発光した送信信号である位相変調光および環境光を含む光束を通過し、通過した光束を受光部１０２に結像する。なお、自己位置推定装置１０は、レンズ１０１と受光部１０２との間に、光学的なフィルタを備えるようにしてもよい。

受光部１０２は、複数の画素が二次元に配列された構造を有する。複数の画素の数は、例えば１０２４×７６８画素である。受光部１０２は、画素によって受光した光に応じた電荷を発生させて、制御部１０７の制御に応じた所定のタイミングで蓄積する。受光部１０２は、蓄積した電荷に基づく信号を制御部１０７に出力する。なお、信号には、送信装置２０のＩＤとタイムコードと同期クラスが、少なくとも含まれている。

ＧＮＳＳ１０３は、衛星から受信した信号から基準信号を抽出し、抽出した基準信号を発振器１０４に出力する。また、ＧＮＳＳ１０３は、受信した信号に基づいて位置（緯度、経度、高度）を計測し、計測した位置情報を位置算出部１２１に出力する。

発振器１０４は、ＧＮＳＳ１０３から入力された基準信号に応じて、生成した同期信号を補正し、補正した同期信号を復調器１０５に出力する。または、発振器１０４は、ＧＮＳＳ１０３が出力した基準信号を用いて同期信号を生成し、生成した同期信号を復調器１０５に出力する。

復調器１０５は、受光部１０２が出力する受信信号に対して、送信装置２０の変調器２０９が用いる変調方式に応じた復調を行い、復調した受信信号を復号器１０６に出力する。なお、画素の制御は、復調器１０５が行うようにしてもよい。
復号器１０６は、復調器１０５が復調した受信信号を、送信装置２０の符号器２０８が用いる符号化に応じて復号し、復号した受信信号を制御部１０７と第１座標探索部１１３に出力する。

制御部１０７は、受光部１０２が有する画素を制御する。また、制御部１０７は、投光器１１０を制御する。制御部１０７は、記憶部１１１が記憶する情報を用いて送信情報を生成し、生成した送信情報を符号器１０８に出力する。制御部１０７は、復号器１０６が出力する送信装置２０からの送信情報を受信し、受信した送信情報から、ＩＤ、タイムコード、同期クラス、情報、および搬送波の位相情報を抽出する。制御部１０７は、抽出したこれらの情報を記憶部１１１に逐次記憶する。制御部１０７は、送信装置２０から送信情報を受信したことに応じて、受信した搬送波の周波数と同じ周波数の変調光に、受信したときの位相情報、自端末のＩＤとタイムコードと同期クラスを含む信号を生成し、生成した信号を符号器１０８に出力する。制御部１０７は、復号器１０６が出力する送信装置２０のＩＤとタイムコードと同期クラスと遅れ時間と情報を取得する。制御部１０７は、２次元の面である受光部の位置情報を角度情報に変換してから、距離情報も利用して３次元の位置情報に変換して、車両間の３次元の相対位置情報を求める。

符号器１０８は、制御部１０７が出力する信号を送信情報とし、送信情報を符号化してビット列を生成する。符号器１０８は、生成したビット列を変調器１０９に出力する。
変調器１０９は、符号器１０８が出力するビット列を、例えばＤＢＰＳＫ方式に従って、発振器１０４が出力する同期信号を用いて変調して搬送波を生成する。変調器１０９は、生成した搬送波を投光器１１０に出力する。

投光器１１０は、変調器１０９が生成した搬送波に基づいて位相変調し、位相変調した送信信号である位相変調光を制御部１０７の制御に応じて送信装置２０へ送信する。投光器１１０は、例えば高レート（繰り返し周波数）の例えば可視光パルスを送信することのできる発光ダイオードまたはレーザダイオードを備える。また、投光器１１０は、高レートの赤外線パルスを送信する発光ダイオードまたはレーザダイオードを備えていてもよい。

記憶部１１１は、自己位置推定装置１０のＩＤと遅れ時間とタイムコード、送信装置２０のＩＤと遅れ時間とタイムコードと同期誤差、符号化、変調、復号化、復調に必要な情報を記憶する。また、記憶部１１１は、受光部１０２を介して撮像したキャリブレーションの結果を記憶する。なお、キャリブレーションについては、後述する。

座標ＤＢ１１２は、送信装置２０のＩＤに、送信装置２０が設置されている位置情報（緯度、経度、高度）を対応付けて格納するデータベースである。以下、本実施形態では、緯度方向をｘ軸方向、経度方向をｙ軸方向、高度方向をｚ軸方向とする。なお、自己位置推定装置１０は、座標ＤＢ１１２を備えていなくてもよい。この場合、座標ＤＢ１１２をサーバー装置（不図示）が備え、自己位置推定装置１０とサーバー装置とがネットワークを介して接続されていてもよい。

第１座標探索部１１３は、復号器１０６が出力する受信信号から送信装置２０のＩＤを抽出する。第１座標探索部１１３は、抽出した送信装置２０のＩＤに対応する位置情報を、座標ＤＢ１１２を探索し、探索した位置情報に送信装置２０のＩＤに対応付けて位置算出部１２１へ出力する。

センサー部１１４は、自己位置推定装置１０の状態を検出し、検出した検出結果を姿勢推定部１１５と相対変位推定部１１６へ出力する。ここで、自己位置推定装置１０の状態とは、自己位置推定装置１０における加速度、角速度、および移動距離を示す情報である。

加速度センサー１３１は、例えば３軸のセンサーであり、自己位置推定装置１０におけるＸＹＺ方向の加速度を検出し、検出した加速度を示す情報を姿勢推定部１１５へ出力する。
レートセンサー１３２は、ジャイロセンサーであり、自己位置推定装置１０の角速度を検出し、検出した角速度を示す情報を姿勢推定部１１５へ出力する。
距離計１３３は、自己位置推定装置１０を搭載する車両の移動距離を計測し、計測した移動距離を示す情報を相対変位推定部１１６に出力する。

姿勢推定部１１５は、加速度センサー１３１が出力する加速度を示す情報と、レートセンサー１３２が出力する角速度を示す情報を用いてピッチ角、ロール角、ヨー角を検出し、検出したピッチ角、ロール角、ヨー角それぞれを示す情報を相対変位推定部１１６へ出力する。

相対変位推定部１１６は、姿勢推定部１１５が出力するピッチ角、ロール角、ヨー角それぞれを示す情報、距離計１３３が出力する移動距離を示す情報を取得する。相対変位推定部１１６は、姿勢推定部１１５が出力する角度と、距離計１３３が出力する移動距離を積分していくことで、慣性航法を行う。相対変位推定部１１６は、前回検出した自己位置と姿勢から現在の位置と姿勢を推定し、推定した位置と姿勢それぞれを示す情報を位置算出部１２１に出力する。なお、姿勢を示す情報は、少なくともピッチ角とロール角を含む。

撮像部１１７は、例えば車両の前方に取り付けられている。撮像部１１７は、車両の前方の画像を撮像し、撮像した撮像画像を車線位置計測部１１８に出力する。撮像画像は、例えば、送信装置２０、車道の車線等を含む。

車線位置計測部１１８は、撮像部１１７が出力する撮像画像の中から、例えば特開平１１−２１９４３５号公報に記載の手法によって白線検出と、白線に対応する画素上の座標を算出する。車線位置計測部１１８は、算出した白線に対応する画素上の座標を第２座標探索部１２０に出力する。

座標ＤＢ１１９は、道路の構造や位置を、所定の形式でデータ化して格納している。例えば、座標ＤＢ１１９は、道路の中心線が１ｍごとに緯度と経度と高度で計測されたデータを格納する。なお、自己位置推定装置１０は、座標ＤＢ１１９を備えていなくてもよい。この場合、座標ＤＢ１１９をサーバー装置（不図示）が備え、自己位置推定装置１０とサーバー装置とがネットワークを介して接続されていてもよい。

第２座標探索部１２０は、第２座標探索部１２０が出力する白線に対応する画素上の座標を用いて、座標ＤＢ１１９が格納するデータを探索して車線（白線）の座標（緯度と経度と高度）を求める。第２座標探索部１２０は、求めた車線の座標を位置算出部１２１に出力する。

位置算出部１２１は、ＧＮＳＳ１０３が出力する位置情報、相対変位推定部１１６が出力する位置と姿勢それぞれを示す情報、第１座標探索部１１３が出力する位置情報に送信装置２０のＩＤに対応付けた情報、第２座標探索部１２０が出力する車線の座標を取得する。位置算出部１２１は、取得した情報に基づいて、現在位置の算出に用いるアルゴリズムを選択する。位置算出部１２１は、選択したアルゴリズムを用いて、現在位置を算出する。なお、アルゴリズムの選択と現在位置の算出方法については、後述する。

＜通信システム１の処理＞
次に、通信システム１の処理手順の一例を説明する。
図２は、本実施形態に係る通信システム１の処理のフローチャートである。

（ステップＳ１）送信装置２０は、送信信号を送信する。続けて、自己位置推定装置１０の制御部１０７は、露光することで送信信号を含む画像を取得しつつ、撮像部１１７を制御して車両１２の前方の画像を撮像する。
（ステップＳ２）制御部１０７は、復号器１０６が出力する信号を取得し、取得した信号を解読する。なお、信号の解読は、復号器１０６が行うようにしてもよい。

（ステップＳ３）制御部１０７は、取得した画像、撮像した画像に基づいて、現在位置の座標が計算可能であるか否か判別する。制御部１０７は、現在位置の座標が計算可能であると判別した場合（ステップＳ３；ＹＥＳ）、ステップＳ４の処理に進み、現在位置の座標が計算可能ではないと判別した場合（ステップＳ３；ＮＯ）、ステップＳ１に処理を戻す。

（ステップＳ４）位置算出部１２１は、受光部１０２によって取得した情報と画像、撮像部１１７が撮像した画像に基づいて、現在位置の座標の計算に用いるアルゴリズムの選択処理を行う。
（ステップＳ５）位置算出部１２１は、ステップＳ４で選択したアルゴリズムを用いて、現在位置の座標の計算を行う。

＜アルゴリズムの選択＞
次に、位置算出部１２１が図２のステップＳ４で行うアルゴリズムの選択手順の一例を説明する。
図３および図４は本実施形態に係るアルゴリズムの選択のフローチャートである。

（ステップＳ１０１）位置算出部１２１は、現在位位置の計算に座標が既知の光源を３つ用いるか否かを判別する。なお、位置算出部１２１は、例えば、ステップＳ１（図２）で取得できた光源の画像数、取得できた光源の信号出力レベル、および取得できた信号の輝度レベルのうち少なくとも１つを用いて、現在位位置の計算に座標が既知の光源を３つ用いるか否かを判別する。位置算出部１２１は、現在位位置の計算に座標が既知の光源を３つ用いると判別した場合（ステップＳ１０１；ＹＥＳ）、ステップＳ１０２に処理を進め、現在位位置の計算に座標が既知の光源を３つ用いないと判別した場合（ステップＳ１０１；ＮＯ）、ステップＳ１０８に処理を進める。

（ステップＳ１０２）位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いるか否かを判別する。なお、自己の姿勢とは、車両の姿勢である。また、位置算出部１２１は、姿勢推定部１１５が出力する角度のうちピッチ角とロール角を用いる。位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いると判別した場合（ステップＳ１０２；ＹＥＳ）、ステップＳ１０３に処理を進め、現在位置の計算に自己の姿勢情報を用いないと判別した場合（ステップＳ１０２；ＮＯ）、ステップＳ１０４に処理を進める。

（ステップＳ１０３）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第１アルゴリズムを適用することを決定する。なお、第１アルゴリズムについては、後述する。

（ステップＳ１０４）位置算出部１２１は、現在位置の計算に光源との距離情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源との距離情報を用いると判別した場合（ステップＳ１０４；ＹＥＳ）、ステップＳ１０５に処理を進め、現在位置の計算に光源との距離情報を用いないと判別した場合（ステップＳ１０４；ＮＯ）、ステップＳ１０６に処理を進める。

（ステップＳ１０５）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第２アルゴリズムを適用することを決定する。なお、第２アルゴリズムについては、後述する。
（ステップＳ１０６）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第３アルゴリズムを適用することを決定する。なお、第３アルゴリズムについては、後述する。

（ステップＳ１０８）位置算出部１２１は、現在位位置の計算に座標が既知の光源を２つ用いるか否かを判別する。なお、位置算出部１２１は、例えば、ステップＳ１（図２）で取得できた光源の画像数、取得できた光源の信号出力レベル、および取得できた信号の輝度レベルのうち少なくとも１つを用いて、現在位位置の計算に座標が既知の光源を２つ用いるか否かを判別する。位置算出部１２１は、現在位位置の計算に座標が既知の光源を２つ用いると判別した場合（ステップＳ１０８；ＹＥＳ）、ステップＳ１０９に処理を進め、現在位位置の計算に座標が既知の光源を２つ用いないと判別した場合（ステップＳ１０８；ＮＯ）、ステップＳ１２１（図４）に処理を進める。

（ステップＳ１０９）位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いると判別した場合（ステップＳ１０９；ＹＥＳ）、ステップＳ１１０に処理を進め、現在位置の計算に自己の姿勢情報を用いないと判別した場合（ステップＳ１０９；ＮＯ）、処理を終了する。

（ステップＳ１１０）位置算出部１２１は、現在位置の計算に自己の高度情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に自己の高度情報を用いると判別した場合（ステップＳ１１０；ＹＥＳ）、ステップＳ１１１に処理を進め、現在位置の計算に自己の高度情報を用いないと判別した場合（ステップＳ１１１；ＮＯ）、ステップＳ１１２に処理を進める。なお、位置算出部１２１は、ＧＮＳＳ１０３が出力する位置情報の中から高度を抽出する。

（ステップＳ１１１）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第４アルゴリズムを適用することを決定する。なお、第４アルゴリズムについては、後述する。

（ステップＳ１１２）位置算出部１２１は、現在位置の計算に光源である送信装置２０との距離情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源との距離情報を用いると判別した場合（ステップＳ１１２；ＹＥＳ）、ステップＳ１１３に処理を進め、現在位置の計算に光源との距離情報を用いないと判別した場合（ステップＳ１１２；ＮＯ）、ステップＳ１１４に処理を進める。

（ステップＳ１１３）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第５アルゴリズムを適用することを決定する。なお、第５アルゴリズムについては、後述する。

（ステップＳ１１４）位置算出部１２１は、現在位置の計算に光源である送信装置２０の方位情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源の方位情報を用いると判別した場合（ステップＳ１１４；ＹＥＳ）、ステップＳ１１５に処理を進め、現在位置の計算に光源の方位情報を用いないと判別した場合（ステップＳ１１４；ＮＯ）、処理を終了する。

（ステップＳ１１５）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第６アルゴリズムを適用することを決定する。なお、第６アルゴリズムについては、後述する。

図４を用いて説明を続ける。
（ステップＳ１２１）位置算出部１２１は、現在位位置の計算に座標が既知の光源を１つ用いるか否かを判別する。なお、位置算出部１２１は、例えば、ステップＳ１（図２）で取得できた光源の画像数、取得できた光源の信号出力レベル、および取得できた信号の輝度レベルのうち少なくとも１つを用いて、現在位位置の計算に座標が既知の光源を１つ用いるか否かを判別する。位置算出部１２１は、現在位位置の計算に座標が既知の光源を１つ用いると判別した場合（ステップＳ１２１；ＹＥＳ）、ステップＳ１２２に処理を進め、現在位位置の計算に座標が既知の光源を１つ用いないと判別した場合（ステップＳ１２１；ＮＯ）、処理を終了する。

（ステップＳ１２２）位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に自己の姿勢情報を用いると判別した場合（ステップＳ１２２；ＹＥＳ）、ステップＳ１２３に処理を進め、現在位置の計算に自己の姿勢情報を用いないと判別した場合（ステップＳ１２３；ＮＯ）、処理を終了する。

（ステップＳ１２３）位置算出部１２１は、現在位置の計算に車線内位置の情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に車線内位置の情報を用いると判別した場合（ステップＳ１２３；ＹＥＳ）、ステップＳ１２４に処理を進め、現在位置の計算に車線内位置の情報を用いないと判別した場合（ステップＳ１２３；ＮＯ）、ステップＳ１２８に処理を進める。

（ステップＳ１２４）位置算出部１２１は、現在位置の計算に光源である送信装置２０との仰俯角（上下角ともいう）情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源との仰俯角情報を用いると判別した場合（ステップＳ１２４；ＹＥＳ）、ステップＳ１２５に処理を進め、現在位置の計算に光源との仰俯角情報を用いないと判別した場合（ステップＳ１２４；ＮＯ）、ステップＳ１２６に処理を進める。なお、本実施形態において、仰俯角とは、車両のが走行している面に対する垂直方向の光源からの入射角である。

（ステップＳ１２５）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第７アルゴリズムを適用することを決定する。なお、第７アルゴリズムについては、後述する。

（ステップＳ１２６）位置算出部１２１は、現在位置の計算に光源である送信装置２０との距離情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源との距離情報を用いると判別した場合（ステップＳ１２６；ＹＥＳ）、ステップＳ１２７に処理を進め、現在位置の計算に光源との距離情報を用いないと判別した場合（ステップＳ１２６；ＮＯ）、処理を終了する。

（ステップＳ１２７）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第８アルゴリズムを適用することを決定する。なお、第８アルゴリズムについては、後述する。

（ステップＳ１２８）位置算出部１２１は、現在位置の計算に自己の方位情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に自己の方位情報を用いると判別した場合（ステップＳ１２８；ＹＥＳ）、ステップＳ１２９に処理を進め、現在位置の計算に自己の方位情報を用いないと判別した場合（ステップＳ１２８；ＮＯ）、処理を終了する。

（ステップＳ１２９）位置算出部１２１は、現在位置の計算に自己の高度情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に自己の高度情報を用いると判別した場合（ステップＳ１２９；ＹＥＳ）、ステップＳ１３０に処理を進め、現在位置の計算に自己の高度情報を用いないと判別した場合（ステップＳ１２９；ＮＯ）、ステップＳ１３１に処理を進める。

（ステップＳ１３０）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第９アルゴリズムを適用することを決定する。なお、第９アルゴリズムについては、後述する。

（ステップＳ１３１）位置算出部１２１は、現在位置の計算に光源である送信装置２０との距離情報を用いるか否かを判別する。位置算出部１２１は、現在位置の計算に光源との距離情報を用いると判別した場合（ステップＳ１３１；ＹＥＳ）、ステップＳ１３２に処理を進め、現在位置の計算に光源との距離情報を用いないと判別した場合（ステップＳ１３２；ＮＯ）、処理を終了する。

（ステップＳ１３２）位置算出部１２１は、現在位置の座標の計算に用いるアルゴリズムに第１０アルゴリズムを適用することを決定する。なお、第１０アルゴリズムについては、後述する。
以上で、位置算出部１２１は、アルゴリズムの選択処理を終了する。

なお、位置算出部１２１は、相対変位推定部１１６が出力する現在の位置と姿勢を示す情報を初期値とし、上述したアルゴリズムの選択処理を行うことで、収束時間を短縮したり、複数のアルゴリズムが選択され、各アルゴリズムから算出された解である車両の現在位置Ｐの中から１つを選択するようにしてもよい。または、位置算出部１２１は、複数の解が得られた場合、予め設定されている優先順位に応じて１つを選択するようにしてもよい。または、位置算出部１２１は、複数の解が得られた場合、複数の解の平均値を算出するようにしてもよい。または、位置算出部１２１は、複数のアルゴリズムを選択し、得られた解に対して、予め定めてある重み付け値を用いて平均値を算出するようにしてもよい。
または、位置算出部１２１は、複数のアルゴリズムを選択し、得られた解に対して、例えばｋ−平均アルゴリズムを用いてクラスタリングを行う。位置算出部１２１は、クラスタリングを行った結果に基づいて、クラスタから外れるデータをノイズが多く、妥当では無いとして除外するようにしてもよい。

なお、上述した例において、位置算出部１２１は、どの情報を用いるか否かを、得られた情報を用いて判別するようにしてもよく、得られた情報のうち、予め定められている基準（例えば、輝度、信号のＳＮ比、信号が連続して得られている時間）以上の情報を選択して用いるようにしてもよい。

以上のように、本実施形態によれば、自己位置推定装置１０が取得した情報（受光部１０２を解して撮像した画像、センサー部１１４が検出した検出値、撮像部１１７が撮像した画像、ＧＮＳＳ１０３が取得した情報）を用いて、現在位置を算出可能なアルゴリズムを選択するようにした。これにより、従来技術では光源の数が３つないと位置を算出できなかったが、本実施形態によれば、光源の数が２つまたは１つであっても位置を算出することができる。すなわち、本実施形態によれば、光通信において自己位置算出に利用できる送信信号の数が変動した場合であっても、自己位置を適度に求めることができる。

なお、上述した例では、光源である送信装置２０からの送信信号が１〜３つ得られた場合を説明したが、これに限られない。取得する光源からの送信信号の数は、４つ以上であってもよい。この場合、位置算出部１２１は、例えば、４つ以上の送信信号の中から、距離の近い順に３つの送信信号を選択して用いるようにしてもよい。または、４つ以上の送信信号の中から、１〜３つの送信信号を選択し、選択した送信信号を用いて各アルゴリズムで現在位置を算出し、前述したように、算出した複数の値を用いて、予め定められている優先度に基づいて選択、平均値、重み付けした後の平均値、クラスタリング処理等を行うようにしてもよい。

以上のように、本実施形態によれば、複数のアルゴリズムによって算出された複数の自己位置に基づいて、適度な精度にて自己位置を推定することができる。
また、本実施形態によれば、４つ以上の送信信号を自己位置算出に利用できる場合であっても、４つ以上の送信信号から３つ以下の送信信号を選択して、選択した送信信号に基づくアルゴリズムを選択することができる。これにより、本実施形態によれば、４つ以上の送信信号が受信できた場合であっても、適度な精度にて自己位置を推定することができる。

＜送信装置２０と自己位置推定装置１０を搭載する車両１２との間の距離の算出＞
次に、送信装置２０と自己位置推定装置１０を搭載する車両１２との間の距離の算出方法について説明する。
図５は、本実施形態に係る送信装置２０と自己位置推定装置１０を搭載する車両１２との間の相互通信を示す図である。図５において、縦軸は信号がＨ（ハイ）レベルであるかＬ（ロー）レベルであるかを表し、横軸は時刻を表す。また、図５に示す例は、車両１２の進行方向に対して車両１２の前方に、送信装置２０が設置されているとする。
波形ｇ１は、送信装置２０が発光した送信信号を表す。波形ｇ２は、送信装置２０が発光した送信信号を、車両１２の自己位置推定装置１０が受信した受信信号を表す。波形ｇ３は、車両１２の自己位置推定装置１０が発光した送信信号を表す。波形ｇ４は、車両１２の自己位置推定装置１０が発光した送信信号を、送信装置２０が受信した受信信号を表す。

時刻ｔ_１は、送信装置２０が光を発光したタイミングを表す。時刻ｔ_２は、送信装置２０が発光した光を、車両１２の自己位置推定装置１０が受光したタイミングを表す。時刻ｔ_３は、車両１２の自己位置推定装置１０が光を発光したタイミングを表す。時刻ｔ_４は、車両１２の自己位置推定装置１０が発光した光を、送信装置２０が受光したタイミングを表す。
また、ｔは、真の遅れ時間を表す。ｂは、車両１２の自己位置推定装置１０の基準時刻を用いて測定した遅れ時間を表す。ｄは、同期誤差を表す。なお、本実施形態では、同期誤差を位相誤差ともいう。ａは、送信装置２０の基準時刻を用いて測定した遅れ時間を表す。なお、本実施形態では、ａとｂを見かけの遅れ時間ともいう。
図５に示すように、真の遅れ時間ｔは、次式（１）、次式（２）のように表される。

ｂ＋ｄ＝ｔ …（１）

ａ−ｄ＝ｔ …（２）

式（１）、式（２）より、ａ＋ｂ＝２ｔとなり、真の遅れ時間ｔは、次式（３）のように表される。また、同期誤差ｄは、式（１）、式（２）より、次式（４）のように表される。

ｔ＝（ａ＋ｂ）／２ …（３）

ｄ＝（ａ−ｂ）／２ …（４）

なお、送信信号は光信号であるため、送信装置２０と車両１２の自己位置推定装置１０との車間距離Ｌは、次式（５）を用いて、変換することができる。なお、式（５）において、ｃは光速（２９９，７９２，４５８［ｍ／ｓ］）である。

Ｌ＝ｔ・ｃ …（５）

このように、送信装置２０と自己位置推定装置１０との間で、見かけの遅れ時間（ａ、ｂ）を互いに送信することで、自己位置推定装置１０は、送信装置２０と車両１２の間の距離を算出することができる。そして、自己位置推定装置１０は、算出した同期誤差ｄと、光通信毎に求めた見かけの遅れ時間とを用いて、式（１）または式（２）によって、真の遅れ時間ｔを算出することができる。なお、自己位置推定装置１０は、見かけの遅れ時間と同期誤差を、投光器１１０と受光部１０２を介して光通信によって送受信する。なお、自己位置推定装置１０および送信装置２０は、無線通信部（不図示）を備える場合、見かけの遅れ時間と同期誤差を無線通信によって送受信するようにしてもよい。

なお、以下の説明では、第１の送信装置２０−１の位置Ａを（Ｘ_１，Ｙ_１，Ｚ_１）とし、第２の送信装置２０−２の位置Ｂを（Ｘ_２，Ｙ_２，Ｚ_２）とし、第３の送信装置２０−３の位置Ｃを（Ｘ_３，Ｙ_３，Ｚ_３）とし、車両１２の位置Ｐを（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）とする。また、本実施形態では、自己位置推定装置１０を搭載する車両１２の自己位置を原点とし、北方向をｘ軸方向、東方向をｙ軸方向、高度方向をｚ軸方向とする。

＜光源からの光線の入射角度＞
ここで、光源である送信装置２０からの光線の入射角度の算出方法について説明する。
図６は、本実施形態に係る第１アルゴリズムの説明に用いる図である。
本実施形態では、例えば、予め自己位置推定装置１０が、光源の位置と入射光線の角度が既知の光源からの送信信号を、受光部１０２を用いて露光することで撮像する。ここで、角度は、極座標におけるパン角とチルト角である。制御部１０７は、撮像した結果、光源が撮像された全画素中における座標に、撮像時の角度を対応付けて記憶部１１１に記憶する。なお、本実施形態では、このような処理を、カメラキャリブレーションともいう。なお、カメラキャリブレーションは、例えば、特許第３９０６１２３号公報に記載の手法や、特許第４０７７７５５号公報に記載の手法を用いる。

制御部１０７は、車両の走行時または停車時に撮像した画像から、光源から送信された送信信号の座標（位置）を抽出する。例えば，光源が画像上の座標の（１００，２００）に映っていた場合、その画像位置となる入射光線の位置や方向は事前にカメラキャリブレーションによって既知である。制御部１０７は、座標（１００，２００）に対応する記憶部１１１が記憶する情報を参照して、対応する角度情報として極座標（パン角，チルト角）で（−２０°，＋１０°）のように光源の向きを得る。制御部１０７は、同様にして別の光源である送信装置２０の向きを得ることで相対角であるθ_１２およびθ_２３を算出し、算出した角度を示す情報を位置算出部１２１に出力する。

＜第１アルゴリズム＞
次に、第１アルゴリズムについて、図６を用いて説明する。第１アルゴリズムでは、後方交会法を用いる。
第１アルゴリズムにおいては、３つの光源である第１の送信装置２０−１〜第３の送信装置２０−３それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。

まず、制御部１０７は、受光部１０２が撮像した画像から各光源の座標を抽出し、抽出知った座標を用いて、記憶部１１１が記憶する情報を参照して、θ_１２（＝∠ＡＰＢ）と、θ_２３（＝∠ＢＰＣ）を算出する。
ここで、ＡＢＰを頂点とする三角形ΔＡＢＰの外接円の方程式は、Ｘ_１，Ｙ_１，Ｘ_２，Ｙ_２，θ_１２を用いて、次式（６）のように表される。なお、円の半径Ｒ_Ｏとする。

式（６）において、φ_１は線分ＡＢの傾きである。
また、ＢＣＰを頂点とする三角形ΔＢＣＰの外接円の方程式は、Ｘ_１，Ｙ_１，Ｘ_２，Ｙ_２，θ_２３を用いて、次式（７）のように表される。なお、円の半径Ｒ_Ｑとする。

式（７）において、φ_２は線分ＢＣの傾きである。
位置算出部１２１は、三角形ΔＡＢＰの外接円と三角形ΔＢＣＰの外接円との交点を求め、Ｂでない交点をＰに決定することで、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ）の座標を求める。

＜第２アルゴリズム＞
次に、第２アルゴリズムについて説明する。図７は、本実施形態に係る第２アルゴリズムの説明に用いる図である。
第２アルゴリズムにおいては、第１の送信装置２０−１〜第３の送信装置２０−３それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。また、第１の送信装置２０−１〜第３の送信装置２０−３それぞれの位置Ａ，Ｂ，Ｃと、位置Ａと位置Ｐとの距離Ｌ_１と、位置Ｂと位置Ｐとの距離Ｌ_２と、位置Ｃと位置Ｐとの距離Ｌ_３が既知である。なお、制御部１０７は、距離Ｌ_１〜Ｌ_３それぞれを、前述したように光通信を行うことで計測する。

位置算出部１２１は、次式（８）を解いて、位置Ｐ（Ｘ_ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

＜第３アルゴリズム＞
次に、第３アルゴリズムについて説明する。図８は、本実施形態に係る第３アルゴリズムの説明に用いる図である。
第３アルゴリズムにおいては、第１の送信装置２０−１〜第３の送信装置２０−３それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。

位置算出部１２１は、例えば、参考文献１または参考文献２に記載の手法を用いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。この場合、４つの位置候補が算出される。このため、位置算出部１２１は、拘束条件（今までの位置、位置が既知の道路上にいる等）を用いて、４つの候補から１つに絞ることで、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

参考文献１；”Analysis and Solutions of The Three Point erspective Pose Estimation Problem”, Robert M. Haralick ,Chung-nan Lee, Karsten 0ttenberg, Michael Nolle, Computer Vision and Pattern Recognition, 1991. Proceedings CVPR '91. IEEE Computer Society Conference on, IEEE, 1991

参考文献２；”Systems and Replication Review and Analysis of Solutions of the Three Point PerspectivePose Estimation Problem”, ROBERT M. HARALICK, CHUNG-NAN LEE, KARSTEN OTTENBERG, MICHAEL NOLLE, International Journal of Computer Vision, 13, 3, 331-356 , Kluwer Academic Publishers, 1994

＜第４アルゴリズム＞
次に、第４アルゴリズムについて説明する。図９は、本実施形態に係る第４アルゴリズムの説明に用いる図である。
第４アルゴリズムにおいては、第１の送信装置２０−１〜第２の送信装置２０−２それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。また、位置Ｐの高度Ｚ_０と、位置Ａと位置ＰとのＺ軸方向の角度Ａ_１と、位置Ｂと位置ＰとのＺ軸方向の角度Ａ_２が既知である。以下、本実施形態では、角度Ａ_１、角度Ａ_２を仰俯角ともいう。なお、位置算出部１２１は、車両１２の姿勢と受光部１０２によって撮像した画像から得られる光源の方向（パン角、チルト角）から、仰俯角Ａ_１〜Ａ_２それぞれを算出する。

位置算出部１２１は、次式（９）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ）の座標を求める。

＜第５アルゴリズム＞
次に、第５アルゴリズムについて説明する。図１０は、本実施形態に係る第５アルゴリズムの説明に用いる図である。
第５アルゴリズムにおいては、第１の送信装置２０−１〜第２の送信装置２０−２それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。また、位置Ａと位置Ｐとの距離Ｌ_１と、仰俯角Ａ_１と、仰俯角Ａ_２が既知である。なお、制御部１０７は、距離Ｌ_１を光通信によって計測する。また、位置算出部１２１は、仰俯角Ａ_１〜Ａ_２それぞれを前述したように算出する。

位置算出部１２１は、次式（１０）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

＜光源と車両との方位角＞
ここで、光源である送信装置２０と、自己位置推定装置１０を搭載する車両１２との方位角の算出方法について説明する。
図１１は、本実施形態に係る第６アルゴリズムの説明に用いる図である。
自己位置推定装置１０を搭載する車両１２のおおよその位置Ｐ’（Ｘ’ｐ，Ｙ’ｐ，Ｚ’ｐ）が既知であり，位置Ｐ’から速度（Ｖｘ，Ｖｙ）で移動することで時間Δｔが経過し現在の位置が不明な状況であると仮定する。また、図１１において、第３の送信装置２０−３が、第１の送信装置２０−１〜第２の送信装置２０−２より遠方に位置しているとする。
この場合、位置算出部１２１は、太陽、月、星などの天体が見える（撮像される）べき方位を、自己の緯度と経度と時刻から算出することができる。光源が所定の距離より遠い場合、位置算出部１２１は、光源の方位ｄ_３を次式（１１）から算出する。

また、光源の見かけの方向(パン角，チルト角)が位置Ａ、Ｂ、Ｃについてそれぞれ（ａ_１，ｂ_１）、（ａ_２，ｂ_２）、（ａ_３，ｂ_３）であるとする。この場合、位置算出部１２１は、車両１２の方位α（＝ａ_３−ｄ_３）を算出する。

＜第６アルゴリズム＞
次に、第６アルゴリズムについて、図１１を用いて説明する。
第６アルゴリズムにおいては、第１の送信装置２０−１〜第２の送信装置２０−２それぞれの位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置Ｐは未知である。また、位置Ａと位置Ｐとの方位角ｄ_１と、位置Ｂと位置Ｐとの方位角ｄ_２が既知である。

ここで、受光部１０２によって撮像した画像から検出した光源（送信装置２０が送信した光信号）の方向（パン、チルト）が（α，δ）である場合、法線ベクトルの座標（ｘ，ｙ，ｚ）は、（ｃｏｓδｃｏｓα，ｃｏｓδｓｉｎα，ｓｉｎδ）で表すことができる。
位置算出部１２１は、法線ベクトルの座標（ｘ，ｙ，ｚ）を自己位置推定装置１０の傾斜している角度のピッチとロール（ｐ、ｒ）を用いて、次式（１２）のように変換する。

位置算出部１２１は、式（１２）によって算出した座標を、次式（１３）を用いて、受光部１０２によって撮像した画像から検出した光源の方向（α，δ）を補正する。

位置算出部１２１は、光源毎に上記の補正を行って、方位角ｄ_１と方位角ｄ_２を算出する。

そして、位置算出部１２１は、次式（１４）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

＜第７アルゴリズム＞
次に、第７アルゴリズムについて説明する。図１２は、本実施形態に係る第７アルゴリズムの説明に用いる図である。
第７アルゴリズムにおいては、第１の送信装置２０−１の位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置は未知であるが車線１４内である。また、仰俯角Ａ_１が既知である。なお、位置算出部１２１は、仰俯角Ａ_１を前述したように算出する。なお、ここで、車両１２が走行している道路における車線（白線）が既知であるとする。なお、第２座標探索部１２０は、車線の座標を前述したように求める。

位置算出部１２１は、次式（１５）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

＜第８アルゴリズム＞
次に、第８アルゴリズムについて説明する。図１３は、本実施形態に係る第８アルゴリズムの説明に用いる図である。
第８アルゴリズムにおいては、第１の送信装置２０−１の位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置は未知であるが車線１４内である。また、位置Ａと位置Ｐとの距離Ｌ_１が既知である。なお、制御部１０７は、距離Ｌ_１を光通信によって計測する。

位置算出部１２１は、次式（１６）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

＜第９アルゴリズム＞
次に、第９アルゴリズムについて説明する。図１５は、本実施形態に係る第９アルゴリズムの説明に用いる図である。
第９アルゴリズムにおいては、第１の送信装置２０−１の位置が既知である。また、自己位置推定装置１０を搭載する車両１２の位置は未知である。また、位置Ｐの高度Ｚ_０、位置Ａと位置Ｐとの方位角ｄ_１が既知である。なお、位置算出部１２１は、方位角ｄ_１を前述したように算出する。

位置算出部１２１は、次式（１７）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ）の座標を求める。

＜第１０アルゴリズム＞
次に、第１０アルゴリズムについて説明する。図１６は、本実施形態に係る第１０アルゴリズムの説明に用いる図である。
第１０アルゴリズムにおいては、第１の送信装置２０−１の位置と、自己位置推定装置１０を搭載する車両１２の位置が未知である。また、位置Ａと位置Ｐとの方位角ｄ_１と、位置Ａと位置ＰとのＺ軸方向の角度Ａ_１が既知である。なお、位置算出部１２１は、角度Ａ_１を前述したように算出し、方位角ｄ_１を前述したように算出する。

位置算出部１２１は、次式（１８）を解いて、位置Ｐ（Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ）の座標を求める。

以上のように、本実施形態によれば、自己位置算出に利用できる送信信号の数が３つの場合、受信できた３つの送信信号に基づいて、高精度に自己位置を推定することができる。

また、本実施形態によれば、自己位置算出に利用できる送信信号の数が２つの場合、２つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）、位置Ｂ（Ｘ_２，Ｙ_２，Ｚ_２））と、自己位置推定装置１０の姿勢（仰俯角Ａ_１、仰俯角Ａ_２）と、自己位置推定装置１０の高度（Ｚ_０）に基づいて、高精度に自己位置を推定することができる。
また、本実施形態によれば、自己位置算出に利用できる送信信号の数が２つの場合、２つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）、位置Ｂ（Ｘ_２，Ｙ_２，Ｚ_２））と、自己位置推定装置１０の姿勢（仰俯角Ａ_１、仰俯角Ａ_２）と、２つの送信装置２０それぞれと自己位置推定装置１０との距離（Ｌ_１、Ｌ_２）それぞれに基づいて、高精度に自己位置を推定することができる。
また、本実施形態によれば、自己位置算出に利用できる送信信号の数が２つの場合、２つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）、位置Ｂ（Ｘ_２，Ｙ_２，Ｚ_２））と、自己位置推定装置１０の姿勢と、２つの送信装置２０それぞれと自己位置推定装置１０との間の方位角（ｄ_１、ｄ_２）それぞれに基づいて、高精度に自己位置を推定することができる。

また、本実施形態によれば、自己位置算出に利用できる送信信号の数が１つの場合、１つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）と、自己位置推定装置１０の姿勢（Ａ_１）と、自己位置推定装置１０を搭載する車両１２が走行している道路の位置情報（拘束条件）と、送信装置２０と自己位置推定装置１０との仰俯角（Ａ_１）に基づいて、高精度に自己位置を推定することができる。
また、本実施形態によれば、自己位置算出に利用できる送信信号の数が１つの場合、１つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）と、自己位置推定装置１０の姿勢と、自己位置推定装置１０を搭載する車両１２が走行している道路の位置情報（拘束条件）と、１つの送信装置２０と自己位置推定装置１０との距離（Ｌ_１）に基づいて、高精度に自己位置を推定することができる。

また、本実施形態によれば、自己位置算出に利用できる送信信号の数が１つの場合、１つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）と、自己位置推定装置１０の姿勢（Ａ_１）と、自己位置推定装置１０の方位角（ｄ_１）と、自己位置推定装置１０の高度（Ｚ_０）に基づいて、高精度に自己位置を推定することができる。
また、本実施形態によれば、自己位置算出に利用できる送信信号の数が１つの場合、１つの送信装置２０の位置（位置Ａ（Ｘ_１，Ｙ_１，Ｚ_０）と、自己位置推定装置１０の姿勢（Ａ_１）と、自己位置推定装置１０の方位角（ｄ_１）と、１つの送信装置２０と自己位置推定装置１０との距離（Ｌ_１）に基づいて、高精度に自己位置を推定することができる。

なお、上述した各アルゴリズムにおける算出例は、一例であり、位置算出部１２１は、他の算出手法、例えば数値計算による手法等を用いて算出するようにしてもよい。
また、自己位置推定装置１０を搭載する車両は、鞍乗り型車両（自動二輪車）、自動三輪車、四輪車等であってもよい。

なお、本発明における自己位置推定装置１０または送信装置２０が備える機能のうち少なくとも１つを実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ホームページ提供環境（あるいは表示環境）を備えたＷＷＷシステムも含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。

また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

１…通信システム、１０…自己位置推定装置、２０、２０−１〜２０−３…送信装置、１０１…レンズ、１０２…受光部、１０３…ＧＮＳＳ、１０４…発振器、１０５…復調器、１０６…復号器、１０７…制御部、１０８…符号器、１０９…変調器、１１０…投光器、１１１…記憶部、１１２…座標ＤＢ、１１３…第１座標探索部、１１４…センサー部、１１５…姿勢推定部、１１６…相対変位推定部、１１７…撮像部、１１８…車線位置計測部、１１９…座標ＤＢ、１２０…第２座標探索部、１２１…位置算出部、１３１…加速度センサー、１３２…レートセンサー、１３３…距離計、２０１…レンズ、２０２…受光部、２０３…ＧＮＳＳ、２０４…発振器、２０５…復調器、２０６…復号器、２０７…制御部、２０８…符号器、２０９…変調器、２１０…投光器、２１１…記憶部

Claims (4)

1. 送信装置からの発光信号を画素によって受光する受光部と、
   前記受光部が取得した前記発光信号のうち自己位置の算出に用いる数に応じて、前記自己位置を推定する複数のアルゴリズムの中から少なくとも１つを選択し、選択した前記アルゴリズムを用いて前記自己位置を算出する位置算出部と、
   を備え、
   前記位置算出部は、
   前記受光部が４つ以上の前記発光信号を取得した場合、４つ以上の前記発光信号の中から３つ以下の前記発光信号を選択し、選択した前記発光信号を用いて前記複数のアルゴリズムの中から２つ以上の前記アルゴリズムを選択し、予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムの中から少なくとも１つの前記アルゴリズムを選択、または予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムによって算出される前記自己位置の中から少なくとも１つを選択する処理、算出した前記自己位置の平均値を算出する処理、算出した前記自己位置に対して予め定められている重み付けした後に平均を算出する処理、算出した前記自己位置に対してクラスタリングを行い、クラスタに基づいて前記自己位置を算出する処理のうち少なくとも１つの処理によって前記自己位置を算出する、
   自己位置推定装置。
2. 送信装置からの発光信号を画素によって受光する受光部と、
   前記受光部が取得した前記発光信号のうち自己位置の算出に用いる数に応じて、前記自己位置を推定する複数のアルゴリズムの中から少なくとも１つを選択し、選択した前記アルゴリズムを用いて前記自己位置を算出する位置算出部と、
   を備え、
   前記位置算出部は、
   前記発光信号のうち自己位置算出に利用できる数が３つの場合に、
   ３つの前記送信装置の位置と、自己位置推定装置の姿勢とが既知のとき第１アルゴリズムを選択し、３つの前記送信装置の位置と、前記自己位置推定装置の姿勢に基づいて前記自己位置を推定し、
   ３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれが既知のとき第２アルゴリズムを選択し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれに基づいて前記自己位置を推定し、
   ３つの前記送信装置の位置のみが既知のとき第３アルゴリズムを選択し、３つの前記送信装置の位置に基づいて前記自己位置を推定する、
   自己位置推定装置。
3. 受光部が、送信装置からの発光信号を画素によって受光する受光手順と、
   位置算出部が、前記受光部が４つ以上の前記発光信号を取得した場合、４つ以上の前記発光信号の中から３つ以下の前記発光信号を選択し、選択した前記発光信号を用いて複数のアルゴリズムの中から２つ以上の前記アルゴリズムを選択し、予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムの中から少なくとも１つの前記アルゴリズムを選択、または予め定められている優先度に基づいて前記２つ以上の前記アルゴリズムによって算出される自己位置の中から少なくとも１つを選択する処理、算出した前記自己位置の平均値を算出する処理、算出した前記自己位置に対して予め定められている重み付けした後に平均を算出する処理、算出した前記自己位置に対してクラスタリングを行い、クラスタに基づいて前記自己位置を算出する処理のうち少なくとも１つの処理によって前記自己位置を算出する手順と、
   を含む自己位置推定方法。
4. 受光部が、送信装置からの発光信号を画素によって受光する受光手順と、
   位置算出部が、前記発光信号のうち自己位置算出に利用できる数が３つの場合に、３つの前記送信装置の位置と、自己位置推定装置の姿勢とが既知のとき第１アルゴリズムを選択し、３つの前記送信装置の位置と、前記自己位置推定装置の姿勢に基づいて自己位置を推定し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれが既知のとき第２アルゴリズムを選択し、３つの前記送信装置の位置と、３つの前記送信装置それぞれと前記自己位置推定装置との距離それぞれに基づいて前記自己位置を推定し、３つの前記送信装置の位置のみが既知のとき第３アルゴリズムを選択し、３つの前記送信装置の位置に基づいて前記自己位置を算出する手順と、
   を含む自己位置推定方法。

Images