JP5145735B2 - 測位装置及び測位システム - Google Patents

Description

本発明は、測位装置及び測位システムに係り、特に、自装置の位置を計測する測位装置及び測位システムに関する。

従来より、最低限５個の衛星からの信号を用いて、３次元測位を行う衛星測位方法が知られている（特許文献１）。この衛星測位方法では、Ｘ、Ｙ、Ｚ、Ｔの未知数を有する方程式を解くことにより、擬似距離測定を行っている。なお、（Ｘ、Ｙ、Ｚ）が予め定義された座標系における３次元の移動***置を表しており、Ｔが全衛星についての擬似距離を決定する同期測定時刻を表している。

また、位置情報を送信する赤外線送信装置を配置し、対象物に取り付けた赤外線受信装置を用いて位置情報を取得する位置検出装置が知られている（特許文献２）。この位置検出装置では、赤外線送信装置によって、照射方向によって異なる情報を発信し、対象となる空間を分割する。空間をより細かく分割するためには、高密度可動鏡面デバイスを用いることが有効であり、高精細でありながら信号同士の重なりが少ない照射ができる。この位置検出装置を複数台用いることで、３次元の位置特定も可能になる。
特開２００６−２８４４５２ 特開２００６−２５０５５４

しかしながら、上記の特許文献１に記載の技術では、高層ビルや立体交差などの高架道路が存在する都心部において、必要な数のＧＰＳ衛星を捉えられない場合があるため、測位できない状況が発生してしまい、安定して測位することができない、という問題がある。また、高層ビルなどで電波が反射されるマルチパスの影響により、測位精度が低下してしまう、という問題がある。

また、上記の特許文献２に記載の技術では、分割された空間の単位で位置を計測するため、高精度な測位を実現することが難しい、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、安定して、高精度に自装置の位置を計測することができる測位装置及び測位システムを提供することを目的とする。

上記の目的を達成するために本発明に係る測位装置は、二次元配列された複数の受光素子を備え、存在位置を示す光信号を送信する光源から前記光源の存在位置を示す光信号を受信するための光受信手段と、前記光受信手段によって受信した光信号に基づいて、前記光源が存在する方向を検出する方向検出手段と、所定期間における自装置の移動距離を検出する移動検出手段と、前記光受信手段によって、少なくとも３つの光源から前記光信号を受信したときに、前記方向検出手段によって検出された前記少なくとも３つの光源の各々が存在する方向、及び前記光受信手段によって受信した前記少なくとも３つの光源からの光信号が示す前記光源の各々の存在位置に基づいて、自装置の位置を計測し、前記光受信手段によって２つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、前記２つの光源の各々が存在する方向のなす角を算出し、算出された前記なす角、前記２つの光源の各々の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測し、前記光受信手段によって１つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、自装置の所定方向に対する前記方向検出手段によって検出された前記１つの光源が存在する方向の相対角度を算出し、前記算出された相対角度、前記１つの光源の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測する測位手段とを含んで構成されている。

また、本発明に係る測位システムは、存在位置を示す光信号を送信する複数の光源と、二次元配列された複数の受光素子を備え、前記光源から前記光信号を受信するための光受信手段、前記光受信手段によって受信した光信号に基づいて、前記光源が存在する方向を検出する方向検出手段、所定期間における自装置の移動距離を検出する移動検出手段、及び前記光受信手段によって、少なくとも３つの光源から前記光信号を受信したときに、前記方向検出手段によって検出された前記少なくとも３つの光源の各々が存在する方向、及び前記光受信手段によって受信した前記少なくとも３つの光源からの光信号が示す前記光源の各々の存在位置に基づいて、自装置の位置を計測し、前記光受信手段によって２つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、前記２つの光源の各々が存在する方向のなす角を算出し、算出された前記なす角、前記２つの光源の各々の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測し、前記光受信手段によって１つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、自装置の所定方向に対する前記方向検出手段によって検出された前記１つの光源が存在する方向の相対角度を算出し、前記算出された相対角度、前記１つの光源の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測する測位手段を含む測位装置とを含んで構成されている。

本発明によれば、複数の光源によって、光源の存在位置を示す光信号を送信し、測位装置において、光受信手段によって、光源から光信号を受信する。このとき、二次元配列された複数の受光素子の何れかによって、光信号を受光する。

そして、測位装置の方向検出手段によって、光受信手段によって受信した光信号を受光した受光素子の位置に基づいて、光源が存在する方向を検出する。

また、光受信手段によって、少なくとも３つの光源から光信号を受信したときに、測位手段によって、方向検出手段によって検出された少なくとも３つの光源の各々が存在する方向、及び光受信手段によって受信した少なくとも３つの光源からの光信号が示す光源の各々の存在位置に基づいて、三角測量の原理により、自装置の位置を計測する。

このように、少なくとも３つの光源から存在位置を示す光信号を受信し、３つの光源の各々が存在する方向、及び３つの光源の各々の存在位置に基づいて、自装置の位置を計測することにより、安定して、高精度に自装置の位置を計測することができる。

本発明に係る測位手段は、方向検出手段によって検出された少なくとも３つの光源の各々が存在する方向に基づいて、少なくとも３つの光源のうちの２つの光源の各々が存在する方向のなす角を、２つの光源の少なくとも２つの組み合わせの各々について算出する角度算出手段を備え、光受信手段によって少なくとも３つの光源から光信号を受信したときに、角度算出手段によって算出された各組み合わせのなす角、及び少なくとも３つの光源の各々の存在位置に基づいて、自装置の位置を計測することができる。これにより、三角測量の原理により、２つの光源が存在する方向のなす角と各光源の存在位置とに基づいて、高精度に自装置の位置を計測することができる。

また、本発明に係る測位装置は、測位手段は、自装置の位置を時系列に計測し、光受信手段によって２つの光源のみから光信号を受信したときに、２つの光源の各々が存在する方向のなす角を算出し、前回計測された自装置の位置を中心とし、移動検出手段によって検出された前回計測時から現在までの期間の移動距離を半径とする円上の位置であって、かつ該位置から２つの光源の各々への方向のなす角が算出したなす角となる複数の候補位置を検出し、検出された複数の候補位置から、自装置の所定方向に対する２つの光源が存在する方向の相対角度が方向検出手段によって検出された２つの光源が存在する方向に最も近くなるような位置を求め、求められた位置に基づいて、自装置の位置を計測することができる。これにより、２つの光源のみから光信号を受信した場合であっても、高精度に自装置の位置を計測することができる。

本発明に係る測位装置は、測位手段は、光受信手段によって１つの光源のみから光信号を受信したときに、前回計測された自装置の位置を中心とし、移動検出手段によって検出された前回計測時から現在までの期間の移動距離を半径とする円上の各々の位置から、自装置の所定方向に対する１つの光源が存在する方向の相対角度が方向検出手段によって検出された前記１つの光源が存在する方向に最も近くなるような位置を求め、前記求められた位置に基づいて、自装置の位置を計測することができる。これにより、１つの光源のみから光信号を受信した場合であっても、高精度に自装置の位置を計測することができる。

上記の光受信手段は、光源を撮像することにより、光源からの光信号を受信し、方向検出手段は、光受信手段によって撮像された画像に基づいて、光信号の受信方向を検出することができる。

上記の光源は、信号機、街灯、及び道路鋲の何れかとすることができる。これにより、既存の設備を利用することができるため、低コストで構成することができる。

また、上記の測位装置は、自装置の所定方向に対する方向検出手段によって検出された光源が存在する方向の相対角度を算出する相対角度算出手段と、光源からの光信号が示す光源の存在位置、測位手段によって計測された自装置の位置、及び相対角度算出手段によって算出された相対角度に基づいて、自装置の所定方向の向きを計測する方向計測手段とを更に含むことができる。これにより、自装置の位置と共に、自装置の向きを高精度に計測することができる。

以上説明したように、本発明の測位装置及び測位システムによれば、少なくとも３つの光源から存在位置を示す光信号を受信し、３つの光源の各々が存在する方向、及び３つの光源の各々の存在位置に基づいて、自装置の位置を計測することにより、安定して、高精度に自装置の位置を計測することができる、という効果が得られる。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載した測位装置において、光通信を利用して車両の位置を測定する車両測位システムに本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係る車両測位システム１０は、自装置の位置情報を示す光信号を送信する複数の光源１２と、車両に搭載され、かつ、光源１２からの光信号を受信して、自車両の位置を計測する測位装置１４とを備えている。

光源１２は、例えば、街灯、信号機、道路鋲などの道路環境に既に存在する発光可能なインフラ設備を用いて構成されている。光源１２は、光源１２の位置を示す位置情報が格納されているメモリ１６と、メモリ１６に格納されている位置情報を読み出して、送信信号を生成する信号生成部１８と、例えばＬＥＤから構成され、発光すると共に、高速点滅を利用して光信号を送信する発光部２０と、信号生成部１８によって生成された送信信号を光信号によって送信するように、発光部２０の発光を制御する発光制御部２２とを備えている。なお、位置情報は、予め計測された光源１２の絶対位置（例えば、経度、緯度、及び高度を用いて表される３次元の絶対位置）を示している。

光源１２では、電波の代わりに光を用いる通信を行い、ＬＥＤが出射する光を高速に点滅させて、点灯時間の長さや点灯予定期間中における点灯パルスの位置などを、送信する位置情報に応じて変えることにより、位置情報を光信号で送信する。また、光を用いる通信は、人の目では認識できないほどの高速な光の点滅を利用して情報を送信する技術であり、光を利用するため見通し内での通信に限定されるため、信号の受信方向から通信相手の識別や位置の特定が容易である点が電波通信と比べた場合の利点といえる。また、通常、人間の目は、５０Ｈｚ以上の速さで点灯する光に対しては、残像現像によって、連続して点灯しているように見えるが、受信側で、１００Ｈｚ以上の撮像能力を持つＴＶカメラや受光ダイオードを利用することによって、人間には光源の点灯や消灯の状態が変化していないように見せながら、高速にデータを送信することができる。

なお、光源１２として、既存の発光可能なインフラを利用するのではなく、光源として測位用の装置を別途設けてもよいが、既存の発光可能なインフラを利用した方が、コスト面において問題が少なくて済む。また、光送信機によって発光される光を、可視領域の光としてもよく、また赤外領域の光としてもよい。

また、測位装置１４は、例えば車両の前方を撮像する光受信手段としてのＣＣＤカメラ２６と、ＣＣＤカメラ２６で撮像された画像から光信号を検出し、検出した光信号を復号して、光源１２の絶対位置を取得する位置情報取得部２８と、ＣＣＤカメラ２６で撮像された画像から光信号を検出し、画像中の光信号の検出位置から、ＣＣＤカメラ２６に対する光源１２が存在する方向を取得する方向取得部３０と、位置情報取得部２８によって取得された光源１２の絶対位置、及び方向取得部３０によって取得された光源１２の方向に基づいて、ＣＣＤカメラ２６の位置と撮像方向とを、自車両の位置及び走行方向として算出する位置方向算出部３２と、自車両に搭載されたディスプレイを用いて構成され、ディスプレイに表示されたナビ地図上に、位置方向算出部３２によって算出された自車両の位置及び走行方向を表示する表示部３４とを備えている。

図２に示すように、複数の光源１２は、例えば、道路の周辺に設置されている複数の街灯を用いて構成され、道路上を走行する車両に対して光信号を送信する。また、車両に搭載された測位装置１４は、ＣＣＤカメラ２６によって、複数の光源１２を同時に撮像し、複数の光源１２から送信された光信号を同時に受信する。

次に、第１の実施の形態に係る車両測位システム１０の作用について説明する。

まず、道路周辺に設置された複数の光源１２の各々によって、光源１２の位置情報を示す光信号を、道路上を走行している車両に対して常時送信する。

また、車両に搭載された測位装置１４によって、図３に示す測位処理ルーチンが実行される。まず、ステップ１００において、３つ以上の光源１２から送信された光信号を同時に受信したか否かを判定し、ＣＣＤカメラ２６によって撮像された画像から、３つ以上の光信号が検出されると、ステップ１０２へ進み、上記ステップ１００で受信した３つ以上の光信号の各々を復号して、各光信号に含まれる光源１２の絶対位置情報を取得する。なお、光源１２の絶対位置は、緯度、経度、及び高度のようなグローバルな座標系で表現されている。また、特定の地域（国や自治体）ごとに定められた座標系で表現されていてもよい。以下では、３つの光源１２から光信号を受信した場合を例に説明する。

そして、ステップ１０４において、図４に示すように、ＣＣＤカメラ２６によって撮像された画像内における光信号の検出位置Ｉｉ（ｘｉ、ｙｉ）（ｉ＝１、２、３）を各々取得する。ここで、図４に示すように、ＣＣＤカメラ２６の位置及び各光源１２の位置をｘｙｚ軸からなる空間で定義する。ＣＣＤカメラ２６がｚ軸上に位置し、ｚ軸方向が、ＣＣＤカメラ２６の撮像方向と同一方向である。また、ｙ軸方向が、高度方向と同一方向であり、ｘｚ平面が、経度及び緯度で表される平面と同一平面である。

なお、画像内における光信号の大きさは、光源１２までの位置によって変化するため、上記ステップ１０４では、光信号を表す領域の重心点の座標を算出し、算出された光信号の重心点の座標を、検出位置Ｉｉ（ｘｉ、ｙｉ）とする。

また、３つの光源は、ＣＣＤカメラ２６から見て同一直線上にないことが必要であるため、以下では、画像内において３つの光源が観測されていることを前提に説明する。

次のステップ１０６では、光源１２が存在する方向として、ＣＣＤカメラ２６の撮像方向である基準方向に対する光源１２が存在する方向の相対角度を各々算出する。図５のｘｚ平面への投影図が示すように、ＣＣＤカメラ２６の焦点位置Ｏ（ｘ０、ｙ０、ｚ０）（＝（０、０、ｚ０））と焦点距離ｆとは既知であるため、画像中での光源位置Ｉ１（ｘ１、ｙ１）、Ｉ２（ｘ２、ｙ２）、Ｉ３（ｘ３、ｙ３）から、ｘｚ平面上におけるＣＣＤカメラ２６の撮像方向に対する光源１２が存在する方向の相対角度θ_ｉ（ｉ＝１，２，３）を、以下の（１）式により算出して、方位角とする。
θ_ｉ＝ｔａｎ^−１｛（ｘｉ−ｘ０）／ｆ｝ ・・・（１）

また、画像中での光源位置Ｉ１（ｘ１、ｙ１）、Ｉ２（ｘ２、ｙ２）、Ｉ３（ｘ３、ｙ３）から、ｙｚ平面上におけるＣＣＤカメラ２６の撮像方向に対する光源１２が存在する方向の相対角度φ_ｉ（ｉ＝１，２，３）を、以下の（２）式により算出して、俯角とする。
φ_ｉ＝ｔａｎ^−１｛（ｙｉ−ｙ０）／ｆ｝ ・・・（２）

次のステップ１０８では、上記ステップ１０６で算出された複数の方位角に基づいて、３つの光源１２のうちの２つの光源１２の組み合わせ毎に、ｘｚ平面上における２つの光源１２の各々が存在する方向のなす角θ_１、２、θ_１、３、θ_２、３を算出し、また、上記ステップ１０６で算出された複数の俯角に基づいて、３つの光源１２のうちの２つの光源１２の組み合わせ毎に、ｙｚ平面上における２つの光源１２の各々が存在する方向のなす角φ_１、２、φ_１、３、φ_２、３を算出する。

そして、ステップ１１０において、３つの光源１２の各々の絶対位置に基づいて、３つの光源１２のうちの２つの光源１２の組み合わせ毎に、ｘｚ平面上の光源１２間の距離ｒ_１、２、ｒ_１、３、ｒ_２、３を算出し、また、ｘｚ平面上の光源１２間の距離ｓ_１、２、ｓ_１、３、ｓ_２、３を算出する。

そして、ステップ１１２において、上記ステップ１０８で算出されたｘｚ平面上のなす角θ_１、２、θ_１、３と上記ステップ１１０で算出されたｘｚ平面上の距離ｒ_１、２、ｒ_１、３とに基づいて、三角測量の原理により、以下に示す方程式を解いて、ＣＣＤカメラ２６の位置（経度、緯度）を自車両の位置として算出する。

図６に示すように、ｘｚ平面上において、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ１との距離をｄ１とし、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ２との距離をｄ２とすると、余弦定理によって、以下の（３）式で表される。
ｒ_１、２ ^２＝ｄ_１ ^２＋ｄ_２ ^２−２ｄ_１ｄ_２・ｃｏｓθ_１、２ ・・・（３）

ここで、ｘｚ平面上において、光源１２の位置Ｐ１を原点とし、光源１２の位置Ｐ２をｘ軸上の点とすると、ＣＣＤカメラ２６の位置は、（ｘ、ｚ）で表すことができ、（ｘ、ｚ）は、図６の曲線Ｃ上の点である。また、距離ｄ１、ｄ２を以下の（４）式及び（５）式で表すことができる。

また、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ３との距離をｄ３とすると、以下の（６）式で表される。
ｒ_１、３ ^２＝ｄ_１ ^２＋ｄ_３ ^２−２ｄ_１ｄ_３・ｃｏｓθ_１、３ ・・・（６）

ここで、（ｘ、ｚ）は、図６の曲線Ｄ上の点であり、また、光源１２の位置Ｐ３を（ｘ３、ｚ３）とすると、距離ｄ３を以下の（７）式で表すことができる。

なお、（ｘ３、ｚ３）は、位置Ｐ１〜Ｐ３の絶対位置から求められる位置Ｐ１及び位置Ｐ２に対する位置Ｐ３の相対位置関係に基づいて算出することができる。

上述したように、（３）式及び（６）式がｘ、ｚに関する連立方程式となるため、連立方程式を解くことにより、未知変数（ｘ、ｚ）が算出される。また、ｘ、ｚは、光源１２の位置Ｐ１からの相対位置を表しているため、光源１２の位置Ｐ１（緯度、経度）と算出されたｘ、ｚとに基づいて、ＣＣＤカメラ２６の位置（経度、緯度）が算出される。

そして、ステップ１１３において、上記ステップ１０８で算出されたｙｚ平面上のなす角φ_１、２、φ_１、３、と上記ステップ１１０で算出されたｙｚ平面上の距離ｓ_１、２、ｓ_１、３とに基づいて、三角測量の原理により、以下に示す方程式を解いて、ＣＣＤカメラ２６の位置（高度）を自車両の位置として算出する。

ｙｚ平面上において、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ１との距離をｅ１とし、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ２との距離をｅ２とすると、余弦定理によって、以下の（８）式で表される。
ｓ_１、２ ^２＝ｅ_１ ^２＋ｅ_２ ^２−２ｅ_１ｅ_２・ｃｏｓφ_１、２ ・・・（８）

ここで、光源１２の位置Ｐ１を原点とし、光源１２の位置Ｐ２をｙ軸上の点とすると、ＣＣＤカメラ２６の位置は、（ｙ、ｚ）で表すことができ、距離ｅ１、ｅ２は以下の（９）式及び（１０）式で表される。

また、ＣＣＤカメラ２６の位置と光源１２の位置Ｐ３との距離をｅ３とすると、以下の（１１）式で表される。
ｓ_１、３ ^２＝ｅ_１ ^２＋ｅ_３ ^２−２ｅ_１ｅ_３・ｃｏｓθ_１、３ ・・・（１１）

ここで、光源１２の位置Ｐ３を（ｙ３、ｚ３）とすると、距離ｅ３が以下の（１２）式で表される。

なお、（ｙ３、ｚ３）は、位置Ｐ１〜Ｐ３の絶対位置から求められる位置Ｐ１及び位置Ｐ２に対する位置Ｐ３の相対位置関係に基づいて算出することができる。

上述したように、（８）式及び（１１）式がｙ、ｚに関する連立方程式となるため、連立方程式を解くことにより、未知変数（ｙ、ｚ）が算出される。ｙ、ｚは、光源１２の位置Ｐ１からの相対位置を表しているため、光源１２の位置Ｐ１（高度）と算出されたｚとに基づいて、ＣＣＤカメラ２６の位置（高度）が算出される。

そして、ステップ１１４で、絶対座標系におけるＣＣＤカメラ２６の撮像方向を、自車両の走行方向として算出する。上記ステップ１１４では、上記１０２で取得された光源１２が存在する位置（緯度、経度、高度）の何れか（例えば、位置Ｐ１）と、上記ステップ１１２、１１３で算出されたＣＣＤカメラ２６の位置（緯度、経度、高度）とに基づいて、ＣＣＤカメラ２６の位置から光源１２の位置Ｐ１への絶対座標系（例えば、東西南北）の方向を算出し、算出された絶対座標系の方向、及び上記ステップ１０６で算出されたＣＣＤカメラ２６の撮像方向に対する光源１２が存在する方向の相対角度θ_１、φ_１に基づいて、絶対座標系におけるＣＣＤカメラ２６の撮像方向を、自車両の走行方向として算出する。

次のステップ１１６では、上記ステップ１１２、１１３で算出された位置と、上記ステップ１１４で算出された方向とを、表示部３４のナビ地図上に表示して、ステップ１００へ戻る。なお、このステップ１１６において、算出した高度情報に基づいて、高架の自動車専用道路を走行中なのか、高架下の一般道を走行中なのかを区別して表示することが好ましい。

以上説明したように、第１の実施の形態に係る車両間通信システムによれば、少なくとも３つの光源から存在位置を示す光信号を受信し、３つの光源の各々が存在する方向に基づいて、２つの光源が存在する方向のなす角を組み合わせ毎に算出し、各組み合わせのなす角と３つの光源の各々の存在位置とに基づいて、三角測量の原理により、自車両の位置を計測するため、安定して、高精度に自装置の位置を計測することができる。

また、自車両の位置と共に、自車両の走行方向を高精度に計測することができる。

光源として、信号機、街灯、又は道路鋲を利用するため、既存の道路設備を利用することができ、新たなインフラを必要とせず、低コストで構成することができる。

また、高層ビルや立体交差などの高架によりＧＰＳが十分な性能を発揮できない都心部やトンネル内部であっても、高精度な測位を実現できる。また、高所に設置された信号機や街灯を利用することで、周囲の車両に妨害されることなく、光信号を受信することができるため、より安定して測位することができる。

また、都心部では、道路に街灯や信号機が既に多数存在し、測位不可となる状況が少ないため、安定して測位することができる。

なお、上記の実施の形態では、ＣＣＤカメラを用いて光信号を受信する場合を例に説明したが、これに限定されるものではなく、二次元配列された複数のフォトダイオードを用いて光信号を受信するようにしてもよい。この場合には、二次元配列されたフォトダイオードによって光源からの光信号を受信すると共に、受光量が最も多いフォトダイオードの位置に基づいて、光源が存在する方向を特定すればよい。

また、光源では、メモリに自身の位置情報を記憶し、メモリからの位置情報に基づいて送信信号を生成する場合を例に説明したが、位置情報は時間変化するものではないため、位置情報を示す送信信号（パルス波形）を予め生成してメモリに記憶しておいてもよい。その場合、メモリから送信信号を読み出すため、信号生成部が不要となる。

また、測位処理ルーチンにおいて、３つの光信号が受信されたときに、ステップ１０２以降の処理を行う場合を例に説明したが、ステップ１００、１０２では、光信号の数に関わらず、光信号を受信して復号し、ステップ１０４において３つ以上の光源位置が検出されたときに、３つ以上の光信号を受信したと判断して、ステップ１０６以降の処理を行うようにしてもよい。

また、１台のＣＣＤカメラによって、３つ以上の光源を同時に撮像して、３つの光信号を受信する場合を例に説明したが、車両に取り付けられた複数のＣＣＤカメラで分担して、３つの光源を同時に撮像するようにしてもよい。この場合には、ＣＣＤカメラ間の位置と姿勢とを予め計測しておくことが必要である。

また、２つの方程式から未知変数（ｘ、ｚ）又は未知数（ｙ、ｚ）を求める場合を例に説明したが、３つの方程式から未知変数（ｘ、ｚ）又は未知数（ｙ、ｚ）を求めるようにしてもよい。この場合には、光源位置Ｐ１、Ｐ２についての上記（３）式と、光源位置Ｐ１、Ｐ３についての上記（６）式と、光源位置Ｐ２、Ｐ３についての以下に示す（１３）式とを求め、これら３つの方程式から、最小二乗法を用いて、未知変数（ｘ、ｚ）を求めればよい。
ｒ_２、３ ^２＝ｄ_２ ^２＋ｄ_３ ^２−２ｄ_２ｄ_３・ｃｏｓθ_２、３ ・・・（１３）

また、同様に、３つの方程式から、最小二乗法により未知変数（ｙ、ｚ）を求めればよい。これにより、ＣＣＤカメラによる光源の観測誤差の影響を小さくすることができる。

また、３つ以上の光源が観測された場合には、上記と同様に、方程式の数を増やして、最小二乗法で解くことにより、測位精度を向上させることができる。

次に、第２の実施の形態に係る車両測位システムについて説明する。なお、第１の実施の形態と同様の構成の部分については、同一符号を付して説明を省略する。

第２の実施の形態では、１つ又は２つの光源から送信された光信号を受信した場合であっても、自車両の位置を計測するようになっている点が第１の実施の形態と異なっている。

第２の実施の形態に係る測位装置２１４は、ＣＣＤカメラ２６と、位置情報取得部２８と、方向取得部３０と、前回位置を計測してから今回の計測時までの計測間隔期間における自車両の移動情報（例えば、移動距離）を検出する移動情報検出部２３６と、位置情報取得部２８によって取得された光源１２の絶対位置、方向取得部３０によって取得された光源１２の方向、及び移動情報検出部２３６によって検出された移動情報に基づいて、ＣＣＤカメラ２６の位置及び撮像方向を、自車両の位置及び走行方向として算出する位置方向算出部２３２と、表示部３４とを備えている。

移動情報検出部２３６は、車輪速を検出するセンサ（図示省略）を備え、検出された車輪速を用いて計測間隔期間における移動距離を検出する。加えて、ステアリング角を取得して車両の移動軌跡を検出してもよい。なお、カメラやレーダなど周囲環境を観測するセンサを用いて、周囲の静止物体を観測し続けることによって、移動距離を検出するようにしてもよい。

また、測位装置２１４はメモリ（図示省略）を備え、計測された位置及び方向をメモリに記憶している。

次に、第２の実施の形態に係る測位処理ルーチンについて、図８を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。

まず、ステップ２５０において、光源１２から送信された光信号を受信したか否かを判定し、ＣＣＤカメラ２６によって撮像された画像から、１つ又は複数の光信号が検出されると、ステップ１０２へ進み、上記ステップ２５０で受信した１つ又は複数の光信号を復号し、光信号に含まれる光源１２の絶対位置情報を取得する。

そして、ステップ１０４で、ＣＣＤカメラ２６によって撮像された画像内における光信号の検出位置Ｉｉ（ｘｉ、ｙｉ）を一つ又は複数取得する。

次のステップ１０６では、光源１２が存在する方向として、ＣＣＤカメラ２６の撮像方向である基準方向に対する光源１２が存在する方向の相対角度（方位角θ_ｉ、俯角φ_ｉ）を一つ又は複数算出する。

そして、ステップ２５２において、上記ステップ２５０で同時に受信した光信号の数を判定し、２つの光源１２のみから光信号を同時に受信した場合には、ステップ２５４において、上記ステップ１０６で算出された２つの方位角に基づいて、ｘｚ平面上における２つの光源１２が存在する方向のなす角θ_１、２を算出し、また、上記ステップ１０６で算出された２つの俯角に基づいて、ｙｚ平面上における２つの光源１２の各々が存在する方向のなす角φ_１、２、を算出する。

そして、ステップ２５６において、２つの光源１２の各々の絶対位置に基づいて、ｘｚ平面上の光源１２間の距離ｒ_１、２を算出し、また、ｙｚ平面上の光源１２間の距離ｓ_１、２を算出する。

そして、ステップ２５８において、移動情報検出部２３６で検出された前回計測時からの移動距離ｄｍを取得し、ステップ２６０で、前回計測された自車両の位置をメモリから取得する。

そして、ステップ２６２において、上記ステップ２５４で算出されたｘｚ平面上のなす角θ_１、２と、上記ステップ２５６で算出されたｘｚ平面上の距離ｒ_１、２と、上記ステップ２５８で取得された移動距離ｄｍと、上記ステップ２６０で取得された前回計測された自車両の位置とに基づいて、以下のように、ＣＣＤカメラ２６の位置（経度、緯度）を自車両の位置として算出する。

図９に示すように、ｘｚ平面上において、光源１２の位置Ｐ１、Ｐ２間の距離ｒ_１、２となす角θ_１、２とが決まっているため、ＣＣＤカメラ２６の位置Ｏは、曲線Ｃ上に制約され、また、前回の測位点Ｏｐと移動距離ｄｍとに基づいて、現在のＣＣＤカメラ２６の位置の候補が２点に絞られる。なお、前回の測位点Ｏｐのｘｚ平面上の位置は、前回測定された絶対位置と、光源１２の位置Ｐ１の絶対位置との相対関係によって求めることができる。

そして、候補となる２点の各々において、ＣＣＤカメラ２６の撮像方向に対する光源１２の位置Ｐ１、Ｐ２の各々が存在する方向の相対角度を算出し、上記ステップ１０６で算出された方位角θ_１、θ_２と比較して、算出された相対角度が方位角θ_１、θ_２に近い候補点の座標（ｘ、ｚ）を求める。そして、求められた（ｘ、ｚ）と上記ステップ１０２で取得された光源１２の位置Ｐ１の絶対位置（緯度、経度）とに基づいて、現在のＣＣＤカメラ２６の位置（緯度、経度）を特定する。

また、ステップ２６４では、上記ステップ２５４で算出されたｙｚ平面上のなす角φ_１、２と、上記ステップ２５６で算出されたＹＺ平面上の距離ｓ_１、２と、上記ステップ２５８で取得された移動距離ｄｍと、上記ステップ２６０で取得された前回計測された自車両の位置とに基づいて、以下に説明するように、ＣＣＤカメラ２６の位置（高度）を自車両の位置として算出し、ステップ１１４へ移行する。

ｙｚ平面上において、光源１２の位置Ｐ１、Ｐ２間の距離ｓ_１、２となす角φ_１、２とが決まっているため、前回の測位点Ｏｐと移動距離ｄｍとに基づいて、現在のＣＣＤカメラ２６の位置の候補を２点に絞り、候補の２点の各々において、ＣＣＤカメラ２６の撮像方向に対する光源１２の位置Ｐ１、Ｐ２の各々が存在する方向の相対角度を算出し、上記ステップ１０６で算出された俯角φ_１、φ_２と比較して、算出された相対角度が俯角φ_１、φ_２に近い候補点の座標（ｙ、ｚ）を求める。そして、求められたｚと上記ステップ１０２で取得された光源１２の位置Ｐ１の絶対位置（高度）とに基づいて、現在のＣＣＤカメラ２６の位置（高度）を特定する。

また、上記ステップ２５２において、１つの光源１２のみから光信号を受信したと判定された場合には、ステップ２６６において、移動情報検出部２３６で検出された前回計測時からの移動距離ｄｍを取得し、ステップ２６８で、前回計測された自車両の位置をメモリから取得する。

そして、ステップ２７０において、上記ステップ１０６で算出された方位角θ_１と、上記ステップ２６６で取得された移動距離ｄｍと、上記ステップ２６８で取得された前回計測された自車両の位置とに基づいて、以下に説明するように、ＣＣＤカメラ２６の位置（経度、緯度）を自車両の位置として算出する。

上記ステップ２７０では、図１０に示すように、ｘｚ平面上において、前回の測位点Ｏｐと移動距離ｄｍとに基づいて、現在のＣＣＤカメラ２６の位置の候補が、測位点Ｏｐを中心とし、半径が移動距離ｄｍとする円上の点に絞られる。なお、前回の測位点Ｏｐのｘｚ平面上の位置は、前回測定された絶対位置と、光源１２の位置Ｐ１の絶対位置との相対関係によって求めることができる。

そして、円上の各点において、ＣＣＤカメラ２６の撮像方向に対する光源１２の位置Ｐ１が存在する方向の相対角度を算出し、上記ステップ１０６で算出された方位角θ_１と比較して、算出された相対角度が方位角θ_１に最も近い点の座標（ｘ、ｚ）を求める。そして、求められた（ｘ、ｚ）と上記ステップ１０２で取得された光源１２の位置Ｐ１の絶対位置（緯度、経度）とに基づいて、現在のＣＣＤカメラ２６の位置（緯度、経度）を特定する。

なお、円上の各点において算出する相対角度は、半径方向に対する、円上の点から位置Ｐ１への方向の相対角度によって求められる。

次のステップ２７２では、上記ステップ１０６で算出された俯角φ_１と、上記ステップ２６６で取得された移動距離ｄｍと、上記ステップ２６８で取得された前回計測された自車両の位置とに基づいて、以下に説明するように、ＣＣＤカメラ２６の位置（高度）を自車両の位置として算出して、ステップ１１４へ移行する。

上記ステップ２７０では、ｙｚ平面上において、前回の測位点Ｏｐと移動距離ｄｍとに基づいて、現在のＣＣＤカメラ２６の位置の候補を、測位点Ｏｐを中心とし、半径が移動距離ｄｍとする円上の点に絞り、円上の各点において、ＣＣＤカメラ２６の撮像方向に対する光源１２の位置Ｐ１が存在する方向の相対角度を算出し、上記ステップ１０６で算出された俯角φ_１と比較して、算出された相対角度が俯角φ_１に最も近い点の座標（ｙ、ｚ）を求める。そして、求められたｚと上記ステップ１０２で取得された光源１２の位置Ｐ１の絶対位置（高度）とに基づいて、現在のＣＣＤカメラ２６の位置（高度）を特定する。

また、上記ステップ２５２において、３つ以上の光源１２から光信号を受信したと判定された場合には、ステップ１０８へ移行する。なお、以下では３つの光信号を同時に受信した場合を例に説明する。ステップ１０８では、上記ステップ１０６で算出された複数の方位角に基づいて、ｘｚ平面上における２つの光源１２の各々が存在する方向のなす角θ_１、２、θ_１、３、θ_２、３を算出し、また、上記ステップ１０６で算出された複数の俯角に基づいて、ｙｚ平面上における２つの光源１２の各々が存在する方向のなす角φ_１、２、φ_１、３、φ_２、３を算出する。

そして、ステップ１１０において、ｘｚ平面上の光源１２間の距離ｒ_１、２、ｒ_１、３、ｒ_２、３を算出し、また、ｙｚ平面上の光源１２間の距離ｓ_１、２、ｓ_１、３、ｓ_２、３を算出し、次のステップ１１２において、上記ステップ１０８で算出されたｘｚ平面上のなす角θ_１、２、θ_１、３と上記ステップ１１０で算出されたｘｚ平面上の距離ｒ_１、２、ｒ_１、３とに基づいて、ＣＣＤカメラ２６の位置（経度、緯度）を自車両の位置として算出する。

そして、ステップ１１３において、上記ステップ１０８で算出されたｙｚ平面上のなす角φ_１、２、φ_１、３と上記ステップ１１０で算出されたｙｚ平面上の距離ｓ_１、２、ｓ_１、３とに基づいて、ＣＣＤカメラ２６の位置（高度）を自車両の位置として算出する。

そして、ステップ１１４で、上記１０２で取得された光源１２が存在する位置Ｐ１と、上記ステップ１１２、１１３、ステップ２６２、２６４、及びステップ２７０、２７２のいずれかで算出されたＣＣＤカメラ２６の位置（緯度、経度、高度）とに基づいて、ＣＣＤカメラ２６から光源１２の位置Ｐ１への絶対座標系（例えば、東西南北）の方向を算出し、算出された絶対座標系の方向と、上記ステップ１０６で算出されたＣＣＤカメラ２６の撮像方向に対する光源１２が存在する方向の相対角度θ_１、φ_１とに基づいて、絶対座標系に対するＣＣＤカメラ２６の撮像方向を、自車両の走行方向として算出する。

次のステップ１１６では、上記ステップ１１２、１１３、ステップ２６２、２６４、及びステップ２７０、２７２のいずれかで算出された位置と、上記ステップ１１４で算出された方向とを、表示部３４のナビ地図上に表示して、ステップ２５０へ戻る。

以上説明したように、第２の実施の形態に係る車両測位システムによれば、３つ以上の光源から光信号を受信できず、２つの光源のみから光信号を受信した場合や１つの光源のみから受信した場合であっても、高精度に自装置の位置を計測することができるため、より安定して測位することができる。

なお、上記の実施の形態では、移動情報として移動距離を検出する場合を例に説明したが、移動距離と共に移動方向も検出するようにしてもよい。この場合には、計測間隔期間の移動距離及び移動方向を用いて、現在のＣＣＤカメラの位置を特定することができる。例えば、２つの光信号を受信した場合には、移動距離、移動方向、光源位置Ｐ１、Ｐ２間の距離、及び光源方向のなす角から、現在のＣＣＤカメラの位置の候補点が１点に特定される。また、１つの光信号を受信した場合には、移動距離、移動方向、及び光源方向の相対角度から、現在のＣＣＤカメラの位置の候補点が１点に特定される。

また、移動情報として、走行軌跡を更に検出するようにしてもよい。この場合には、現在の車両の位置と向きとを大まかに予測することができ、予測された大まかな車両の位置と向きとを用いて、高精度に自車両の位置及び方向を計測することができる。

本発明の第１の実施の形態に係る車両測位システムの構成を示す概略図である。 光源と測位装置を搭載した車両とを示すイメージ図である。 本発明の第１の実施の形態に係る測位装置における測位処理ルーチンの内容を示すフローチャートである。 ｘｙｚ空間におけるＣＣＤカメラによる光源の受信状況を示すイメージ図である。 ｘｚ平面におけるＣＣＤカメラの撮像方向に対する光源が存在する方向の相対角度を示すイメージ図である。 ｘｚ平面において、ＣＣＤカメラの位置を算出する方法を説明するためのイメージ図である。 本発明の第２の実施の形態に係る測位装置の構成を示す概略図である。 本発明の第２の実施の形態に係る測位装置における測位処理ルーチンの内容を示すフローチャートである。 ２つの光信号を受信した場合に、ｘｚ平面において、ＣＣＤカメラの位置を算出する方法を説明するためのイメージ図である。 １つの光信号を受信した場合に、ｘｚ平面において、ＣＣＤカメラの位置を算出する方法を説明するためのイメージ図である。

符号の説明

１０ 車両測位システム
１２ 光源
１４、２１４ 測位装置
１６ メモリ
１８ 信号生成部
２０ 発光部
２２ 発光制御部
２６ ＣＣＤカメラ
２８ 位置情報取得部
３０ 方向取得部
３２、２３２ 位置方向算出部
３４ 表示部
２３６ 移動情報検出部

Claims (8)

1. 二次元配列された複数の受光素子を備え、存在位置を示す光信号を送信する光源から前記光源の存在位置を示す光信号を受信するための光受信手段と、
   前記光受信手段によって受信した光信号に基づいて、前記光源が存在する方向を検出する方向検出手段と、
   所定期間における自装置の移動距離を検出する移動検出手段と、
   前記光受信手段によって、少なくとも３つの光源から前記光信号を受信したときに、前記方向検出手段によって検出された前記少なくとも３つの光源の各々が存在する方向、及び前記光受信手段によって受信した前記少なくとも３つの光源からの光信号が示す前記光源の各々の存在位置に基づいて、自装置の位置を計測し、
   前記光受信手段によって２つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、前記２つの光源の各々が存在する方向のなす角を算出し、算出された前記なす角、前記２つの光源の各々の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測し、
   前記光受信手段によって１つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、自装置の所定方向に対する前記方向検出手段によって検出された前記１つの光源が存在する方向の相対角度を算出し、前記算出された相対角度、前記１つの光源の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測する測位手段と、
   を含む測位装置。
2. 前記測位手段は、前記方向検出手段によって検出された少なくとも３つの光源の各々が存在する方向に基づいて、前記少なくとも３つの光源のうちの２つの光源の各々が存在する方向のなす角を、前記２つの光源の少なくとも２つの組み合わせの各々について算出する角度算出手段を備え、前記光受信手段によって少なくとも３つの光源から前記光信号を受信したときに、前記角度算出手段によって算出された各組み合わせのなす角、及び前記少なくとも３つの光源の各々の存在位置に基づいて、自装置の位置を計測する請求項１記載の測位装置。
3. 前記測位手段は、前記光受信手段によって２つの光源のみから前記光信号を受信したときに、前記２つの光源の各々が存在する方向のなす角を算出し、前回計測された自装置の位置を中心とし、前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離を半径とする円上の位置であって、かつ該位置から前記２つの光源の各々への方向のなす角が前記算出したなす角となる複数の候補位置を検出し、前記検出された複数の候補位置から、自装置の所定方向に対する前記２つの光源が存在する方向の相対角度が前記方向検出手段によって検出された前記２つの光源が存在する方向に最も近くなるような位置を求め、前記求められた位置に基づいて、自装置の位置を計測する請求項１記載の測位装置。
4. 前記測位手段は、前記光受信手段によって１つの光源のみから前記光信号を受信したときに、前回計測された自装置の位置を中心とし、前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離を半径とする円上の各々の位置から、自装置の所定方向に対する前記１つの光源が存在する方向の相対角度が前記方向検出手段によって検出された前記１つの光源が存在する方向に最も近くなるような位置を求め、前記求められた位置に基づいて、自装置の位置を計測する請求項１記載の測位装置。
5. 前記光受信手段は、前記光源を撮像することにより、前記光源からの光信号を受信し、
   前記方向検出手段は、前記光受信手段によって撮像された画像に基づいて、前記光信号の受信方向を検出する請求項１〜請求項４のいずれか１項記載の測位装置。
6. 前記光源は、信号機、街灯、及び道路鋲の何れかである請求項１〜請求項５の何れか１項記載の測位装置。
7. 自装置の所定方向に対する前記方向検出手段によって検出された前記光源が存在する方向の相対角度を算出する相対角度算出手段と、
   前記光源からの光信号が示す前記光源の存在位置、前記測位手段によって計測された自装置の位置、及び前記相対角度算出手段によって算出された相対角度に基づいて、自装置の前記所定方向の向きを計測する方向計測手段とを更に含む請求項１〜請求項６の何れか１項記載の測位装置。
8. 存在位置を示す光信号を送信する複数の光源と、
   二次元配列された複数の受光素子を備え、前記光源から前記光信号を受信するための光受信手段、
   前記光受信手段によって受信した光信号に基づいて、前記光源が存在する方向を検出する方向検出手段、
   所定期間における自装置の移動距離を検出する移動検出手段、及び
   前記光受信手段によって、少なくとも３つの光源から前記光信号を受信したときに、前記方向検出手段によって検出された前記少なくとも３つの光源の各々が存在する方向、及び前記光受信手段によって受信した前記少なくとも３つの光源からの光信号が示す前記光源の各々の存在位置に基づいて、自装置の位置を計測し、
   前記光受信手段によって２つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、前記２つの光源の各々が存在する方向のなす角を算出し、算出された前記なす角、前記２つの光源の各々の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測し、
   前記光受信手段によって１つの光源のみから前記光信号を受信したときに、自装置の位置を時系列に計測し、自装置の所定方向に対する前記方向検出手段によって検出された前記１つの光源が存在する方向の相対角度を算出し、前記算出された相対角度、前記１つの光源の存在位置、前回計測された自装置の位置、及び前記移動検出手段によって検出された前回計測時から現在までの期間の移動距離に基づいて、自装置の位置を計測する測位手段を含む測位装置と、
   を含む測位システム。

Images