JP5114514B2

JP5114514B2 - 位置推定装置

Info

Publication number: JP5114514B2
Application number: JP2010039533A
Authority: JP
Inventors: 幹雄板東; 克明田中; 幸博川股; 利幸青木
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 2010-02-25
Filing date: 2010-02-25
Publication date: 2013-01-09
Anticipated expiration: 2030-02-25
Also published as: US20110244881A1; EP2363731A1; CN102192737B; US8260325B2; CN102192737A; EP2363731B1; JP2011174829A

Description

本発明は、屋内外にて移動体の位置を推定するシステムに関する。

これまでにも、複数の移動体の位置を精度良く求めるために、複数の移動体の情報を用いる方法は提案されている。例えば、特開２００３−３３７０２９号公報では、他移動体のＧＰＳ（Global Positioning System）と自移動体のＧＰＳ信号を比較して、共通のＧＰＳ衛星信号を用いることで相対位置を正確に算出し、地図上にマッピングする装置が提案されている。また、特開２００２−３４０５８９号公報では、正確な位置が既知の基地局を設置し、ＧＰＳ端末が観測した擬似距離から、基地局で計測した擬似距離を元に計算した誤差を引いて、補正情報をネットワークに流すことで、位置を補正する方法を示している。また、特開２００９−１５０７２２号公報では、移動体の絶対位置の確率分布を計算し、３体以上の移動体の絶対位置を特定する方法を示している。また、特開２００８−３１２０５４号公報では、信号の周期をある閾値を用いて判別する方法を示しており、これを用いればＧＰＳ信号の誤検出を少なくすることができる。

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

しかし、ＧＰＳ測位精度を向上させるためのリファレンスとなる基地局の設置は、その設置条件を考えなくてはならないことやコストが高くなることが課題となる。そして基地局設置のためのコストを低減させるために、特許文献２に記載の発明のように、補正情報をネットワークに流して位置を補正する場合や、特許文献３に記載の発明のように複数の移動体で得た情報を基に位置を補正する場合でも、実観測場所にて、各端末がＧＰＳ衛星を４つ以上観測できなくては位置推定ができないなど、三角測量の方式にて位置を推定する方法を採る場合は、距離計測が不安定な場所での測位精度が課題となる。

本発明では、ＧＰＳ測位精度を向上させるためのリファレンスとなる基地局の設置をせずに、三角測量による位置決めができたりできなかったりするような、距離計測が不安定な場所での測位精度を向上させることが課題である。

ＧＰＳやその他の三角測量を用いて移動体の位置を推定する装置において、他の移動体やセンタ等と通信手段により、自装置ではまだ得られていない距離計測情報を取得する。そして、距離計測情報を取得した移動体までの相対距離を距離計測手段により計測する。この計測した移動体間の相対距離を、取得した距離計測情報に適切な変換をかけて付与することで、３点からの距離情報が得られたと同等の連立方程式を立式し、この方程式を解いて測位結果を得る。

従来の、三角測量の原理を用いた位置推定方法では、一つの移動体に対して必ず３点以上の場所からの距離が計測できることが条件となっていたが、複数の移動体の情報を用いて、かつ複数の移動体との関係性を反映することで、複数の移動体を含めて観測できる点数の合計を３点以上とすることで、それぞれの位置を計測できるようになり、測位精度を保つことができるようになる。また、３点以上の場所から自身の位置までの距離を計測できる場合でも、距離の差分を取り、その誤差を最小とするように最尤推定することで、３点からの距離を計測する際に生じる誤差を減らすことができるため、単一の移動体による位置推定精度よりも高精度に位置を推定することが可能となる。

位置推定システムの構成図。移動体における処理フロー図。センタにおける処理フロー図。関係性判定手段の構成図。関係性判定手段の処理フロー図。基線ベクトル算出手段の処理フロー図。連鎖判定手段の処理フロー図。信号独立性判定手段の処理フロー図。関係性判定手段を移動体に持った位置推定システムの構成図。移動体における処理フロー図。本発明による位置推定システムの適用例。

以下図面を用いて、本発明を用いた位置推定システムの実施例を説明する。

図１に、本発明を用いた位置推定システムの第一の実施例の構成図を示す。第一の実施例は、鉱山などのように、決まった移動体が決まった走行路を移動する場合で、移動体とセンタあるいは移動体同士の間で通信を頻繁に行うことが可能な環境の下で測位位置の精度を向上させるものである。

この位置推定システムは、移動体の専用装置もしくは移動体に組み込まれることにより実現され、複数の移動体１０１，１１１と、センタ１２１とから構成される。また、移動体１０１，１１１およびセンタ１２１の管理する時刻は共通の時刻で同期が取られているものとする。図１では、簡単な例として移動体１０１と移動体１１１およびセンタ１２１から構成されている場合を示すが、移動体の数はより多くてもかまわない。移動体１０１は、測位信号などのセンサ情報を取得する信号受信手段１０２と、受信した測位信号などのセンサ情報を基に位置を算出する位置姿勢算出手段１０３と、他の移動体と相対距離を計測する距離計測手段１０４と、移動体に搭載されているセンサの情報や位置姿勢算出手段１０３で算出した位置の情報を外部と通信する通信手段１０５と、センタ１２１から配信される位置補正情報を格納しておく位置補正情報記憶手段１０６と、距離計測手段１０４で相対距離を計測する相手の移動体の特徴を認識する特徴認識手段１０７と、受信した測位信号などのセンサ情報や相対距離の計測結果および、その時の時刻を記憶する計測情報記憶領域および、送信途中の値を記憶する中断情報記憶領域を有する内部記憶手段１０８と、を備えて構成される。距離計測手段１０４と特徴認識手段１０７には、ステレオカメラのように一つのデバイスで対象物までの距離と特徴を取得する二つの効果を持つような機器を用いてもよい。また、移動体１１１も移動体１０１と同様の構成である。

センタ１２１は、移動体１０１や移動体１１１などの複数の移動体から得られる距離の計測の可否を含む移動体相互の相対距離の計測結果から、他の移動体との相対位置などの繋がりを判定する関係性判定手段１２２と、関係性判定手段１２２で判定した情報を記憶する移動体分類記憶手段１２３と、位置補正情報を計算して現在位置を把握している各移動体からの現在位置を補正する位置補正手段１２４と、移動体へ位置補正情報を通信する通信手段１２５と、位置補正手段１２４で算出した位置補正情報を記憶する移動体位置補正情報記憶手段１２６と、複数の移動体から得られた情報を記憶する移動体情報記憶手段１２７と、で構成されている。

次に図２に位置推定システムの移動体における処理フローを、また図３に位置推定システムのセンタにおける処理フローを示す。この位置推定システムは端末における処理とセンタにおける処理に大別され、双方の情報を用いて処理することで全体の処理が行われる。

まず、移動体における処理から説明する。この説明では移動体１０１における動作として説明する。移動体１０１では、所定の周期でセンサ等から得られる測位信号などのセンサ情報を信号受信手段１０２により取得する。また、特徴認識手段１０７は、移動体１０１の周囲を捜査して、事前に認識する対象について内部記憶手段１０８に記憶されている他の移動体の特徴を検索する。ここで移動体の特徴とは、それぞれの移動体が持つレーザー反射板や特徴的な模様／マークや、移動体の形状などであり、予め各移動体に対応付けて格納されているものとする。第一の実施例では、他の移動体として移動体１１１の特徴が格納されている。それぞれの特徴を、事前にルール付けられた変換方式で値に変換し、特徴が認識された方向と合わせて特徴量とする。そして、認識した特徴までの相対距離を距離計測手段１０４にて計測する（ステップ２０１）。

次に、ステップ２０２にて、ステップ２０１で取得したセンサ情報とその取得時刻および計測した相対距離や認識した特徴量を計測情報として内部記憶手段１０８の計測情報記憶領域に記憶する。このとき、計測情報記憶領域に前回に記憶した情報が残っていても上書きされる。

次にステップ２０３にて、位置補正情報が位置補正情報記憶手段１０６にあるかどうかを確認し、もしあれば、ステップ２０４に移行してこの位置補正情報を用いて現在の位置を補正する。もし位置補正情報が無ければ、ステップ２０５に移行する。ステップ２０５では、ステップ２０２で記憶した計測情報を用いて位置・姿勢の算出が可能かどうかを判断する。もし可能であればステップ２０６にて位置・姿勢を算出する。不可能であればステップ２０７に移行する。

ステップ２０７では、センタ１２１との通信が可能かどうかを判断する。センタ１２１との通信が不可能であった場合、ステップ２１３に移行して、次の計測情報取得まで待機する。センタ１２１との通信が可能であれば、ステップ２０８にて、後述する内部記憶手段１０８の中断情報記憶領域に前回のステップ２０９からステップ２１１までの処理の間で途中で保留した処理の情報があるかどうかを判断する。無い場合、ステップ２０９にて、ステップ２０２で記憶したセンサ等の情報およびセンサ情報の取得時刻，自移動体の移動体ＩＤ，認識した他移動体との相対距離と特徴量と共に、位置補正情報の受信要求をセンタへ送信する。ステップ２１０では、ステップ２０９でセンタへ送った位置補正情報の要求に対するセンタ１２１からの応答として送られてくる、受信要求を送った移動体のＩＤに対応した位置補正情報を受信する。ステップ２１１ではステップ２１０にて受信した情報を位置補正情報記憶手段１０６に記憶し、ステップ２１３で計測情報取得まで待機する。

ただし、ステップ２０７以降は、その処理中に次の計測情報取得の機会が来ると、いつでも処理を保留してステップ２０１の計測情報取得の処理とステップ２０２の計測情報の記憶処理を行うため、処理が保留されて残った情報を内部記憶手段１０８の中断情報記憶領域に記憶し、ステップ２０１に戻る。そして、後の処理でステップ２０８における処理で内部記憶手段１０８の中断情報記憶領域に保留情報が有ると判断すると、内部記憶手段１０８の中断情報記憶領域に記憶された保留した所から処理を再開する。同様の処理は移動体１１１の位置推定システムでも行われる。

図３にセンタ１２１の処理フローを示す。まず、移動体からの図２に示した処理フローにおけるステップ２０９で送信された情報を取得すると（ステップ３０１）。次にステップ３０２にて、移動体から受信したセンサ情報およびその取得時刻、他移動体の相対距離とその特徴量の情報を移動体情報記憶手段１２７へ記憶する。またこの時点で、移動体情報記憶手段１２７に記憶されている移動体からの情報の内、ある一定時間以上経過した情報は削除する。また、同一の移動体ＩＤを持つ情報が存在する場合は、その情報を上書きする。

次にステップ３０３にて、複数の移動体からの情報が移動体情報記憶手段１２７に記憶されているかどうかを判断する。単体の移動体の情報しかない場合は、ステップ３０８へ移り、移動体位置補正情報記憶手段１２６の予め管理対象として登録されている全ての移動体ＩＤについて“補正情報無し”と記憶する。もし、複数の移動体からの情報がある場合はステップ３０４に移り、関係性判定手段１２２にて複数の移動体の関係性を判定し、関係性のある移動体を一まとめの集団として判断し、次のステップ３０５にて、ステップ３０４からの出力を各移動体ＩＤと共に移動体分類記憶手段１２３に記憶する。

そしてステップ３０６にて、移動体分類記憶手段１２３に記憶された情報に基づいて、位置補正情報が算出可能かどうかを判断する。この時、受信したセンサ情報におくる測位信号の内、独立な測位信号の数と各移動体の求めるべき位置の未知変数の数を求める。ここで求めた独立な測位信号の数と各移動体の求めるべき位置の未知変数の数を比べ、独立な測位信号の数が各移動体の求めるべき位置の未知変数の数よりも少ない場合、位置補正情報の算出は不可能と判断してステップ３０８へ移り、移動体位置補正情報記憶手段１２６の全ての移動体ＩＤについて“補正情報無し”と記憶する。

また、求めた独立な測位信号の数と各移動体の求めるべき位置の未知変数の数を比べて、独立な測位信号の数が各移動体の求めるべき位置の未知変数の数より多いかもしくは同じ数の場合、位置補正情報の算出は可能と判断すれば、ステップ３０７へ移行し、位置補正情報を算出する。ステップ３０７にて位置補正情報が算出されれば、ステップ３０８にて移動体位置補正情報記憶手段１２６において移動体ＩＤ毎に位置補正情報を記憶する。

次にステップ３０９にて移動体から位置補正情報の要求があるかどうかを判断し、要求がある場合はその受信要求を上げている移動体の移動体ＩＤに対応する位置補正情報を、ステップ３１０にて送信する。もし、移動体からの受信要求を受信していない場合には、ステップ３１１にて、次の情報が受信されるまで待機状態に移る。なお、各移動体における処理フローとセンタ１２１における処理フローは並列で動作する。

このように、移動体の位置推定結果をセンタが機会ある毎に補正していくため、ＧＰＳなどの三角測量による位置決めに用いる距離計測が不安定な場所でも位置精度を向上させることができる。

次に移動体における各装置構成について説明する。移動体における信号受信手段１０２では、それぞれの移動体の位置を算出するための信号が受信される。すなわち、三角測量により位置を算出するための信号や移動体の姿勢および速度を検出するためのセンサの信号である。三角測量により位置を算出するための信号は、ＧＰＳ衛星からの航法電波や、ランドマークからの距離情報などである。移動体の姿勢を検出するためのセンサとは、ジャイロセンサや加速度センサなどの相対センサ、および地磁気センサなどの絶対的な方位を示すセンサである。また、速度を検出するためのセンサは、タイヤのエンコーダや、速度情報を含むＧＰＳや、ドップラー速度計などである。

位置姿勢算出手段１０３では、信号受信手段１０２で受信したセンサ等からの情報を基に移動体の位置を算出する。例えば、ＧＰＳ信号に基づく三角測量では、Ｎ基の衛星からの信号を受信し、第ｉ番目の衛星の位置（Ｘｉ，Ｙｉ，Ｚｉ）と、算出したい移動体の位置（ｘ，ｙ，ｚ）と、第ｉ番目の衛星からの距離ｒｉ、およびその誤差ｓから、次の（式１）で表される方程式を連立して解くことで移動体の位置（ｘ，ｙ，ｚ）を算出することができる。

そして、（式１）で算出した位置と共にその測位誤差を考慮して、位置姿勢算出にカルマンフィルタなどの確率モデルフィルタを使用し、受信した信号から最尤な位置推定を行うと共に、その誤差を確率的に見積もる。

移動体の距離計測手段１０４では、センサ等を用いて自移動体から他の移動体まで間の相対距離を計測する。相対距離を計測するためのセンサとしては、ステレオカメラや、レーザーレンジファインダ、ミリ波レーダ等である。

移動体の特徴認識手段１０７では、自移動体の周囲を捜査し、事前に認識する対象として記憶されている他の移動体の特徴を検索し、自移動体に対する方向と共にする。特徴認識を行うためのセンサとしては、カメラや３次元レーザーレンジファインダ等が挙げられる。

移動体の通信手段１０５では、信号受信手段１０２や位置姿勢算出手段１０３，距離計測手段１０４で得られた情報をセンタ１２１に送信する。また、センタ１２１から送られてくる情報を受信する。

位置補正情報記憶手段１０６では、通信手段１０５で受信したセンタ１２１からの位置補正情報を記憶する。ここで記憶された情報は、位置の補正が行われた後に消去される。

次にセンタ１２１における各装置構成について説明する。関係性判定手段１２２では、複数の移動体の関係性を判定することで、関連する移動体を一まとめの集団として分類すると共に、各自の位置座標や各移動体間の相対距離などからそれぞれの移動体の位置を算出する。関係性判定手段１２２の詳細構成を図４に、またそこでの処理の流れを図５に示す。関係性判定手段１２２は、移動体から送信されてきた距離計測手段１０４で得た情報から移動体間の相対距離が判っている移動体同士を繋がっている、相対距離が判っていない移動体同士は繋がっていないと判定する連鎖判定手段４０２と、信号受信手段にて得られたセンサ情報の内、それぞれの測位信号が独立した発信源からの情報であるか、他の測位信号の発信源と同じであるかどうかを判定する信号独立性判定手段４０３と、各移動体同士の相対的な高さを、移動体における信号受信手段と距離計測手段の少なくともどちらか一方の計算結果を用いて算出する基線ベクトル算出手段４０４とにより構成される。

関係性判定手段１２２は、ステップ３０３で複数の移動体を個別に識別する移動体ＩＤおよびそれぞれの移動体から得られるセンサ情報および相対距離・特徴量のデータを受信していることを確認していることが処理開始の前提となる。まず、ステップ５０２にて、移動体情報記憶手段１２７からセンサ情報および相対距離を計測した移動体の移動体ＩＤを取得する。ここでは、既に移動体に対して移動体ＩＤが振られているものとする。次にステップ５０３にて、取得した相対距離および特徴量の方向から、相対距離を計測した移動体の位置を基準地点とし、計測された移動体を未知地点とした場合のこの未知地点までの基線ベクトルを算出する。

この基線ベクトル算出の詳細な処理フローを図６に示す。ステップ５０２にて取得した移動体情報記憶手段１２７に移動体からの情報が記憶されている全ての移動体についてのループ処理（ステップ６００）では、ステップ６０１にて、まだ基線ベクトル算出を行っていない移動体を１つ選び、選んだ移動体ＩＤに対応する特徴量を移動体情報記憶手段１２７から取得する。ステップ６０２では、移動体からの情報に含まれる計測情報の内、認識した特徴の方向についての情報が、移動体の進行方向に対する高さ方向および水平方向の傾きまでを含む情報を持ち、３次元基線ベクトルとして算出可能かを判断する。３次元基線ベクトルの算出は、ステレオカメラや３次元レーザーレンジファインダなどによる計測であれば可能である。３次元基線ベクトルが算出可能であれば、ステップ６０３にて移動体からの情報に含まれる計測情報の内、移動体の姿勢の情報を取得し、その結果と合わせてステップ６０４にて３次元基線ベクトルを算出する。算出した基線ベクトルはステップ６０９にて相対距離情報として、基準とした移動体の移動体ＩＤと共に連鎖判定手段４０２に送出する。

また、ステップ６０２にて、３次元基線ベクトルが算出不可能と判断された場合、ステップ６０５にて、認識した特徴の方向についての情報が移動体の進行方向に対する水平方向の傾きまでの情報を持ち、２次元基線ベクトルでの算出が可能かを判断する。２次元基線ベクトルの算出は、レーザーレンジファインダなどによる計測であれば可能である。２次元基線ベクトルを算出可能であれば、ステップ６０６にて移動体からの情報に含まれる計測情報の内、移動体の姿勢の情報を取得すると共にステップ６０７にて２次元基線ベクトルを算出し、ステップ６０９にて相対距離情報として、基準とした移動体の移動体ＩＤと共に連鎖判定手段４０２に送出する。

また、ステップ６０５にて、２次元基線ベクトルが算出不可能と判断された場合、ステップ６０８にて、基線ベクトルにおける２点間の方向を算出することはあきらめて相対距離のみを算出する。相対距離が既に算出されている場合は、その相対距離を用いる。そしてステップ６０９にて相対距離を基準とした移動体の移動体ＩＤと共に連鎖判定手段４０２に送出する。

以上の処理を、ステップ５０２にて取得した移動体情報記憶手段１２７に移動体からの情報が記憶されている全ての移動体について、未処理の移動体がなくなるまでステップ６００からのループ処理を続ける。

図５の説明に戻り、次にステップ５０４にて、関係性判定手段１２２における連鎖判定手段４０２により、移動体から送信されてきた距離計測手段１０４で得た情報から移動体間の相対距離が判っている移動体同士を繋がっている、相対距離が判っていない移動体同士は繋がっていないと判定する。この連鎖判定手段４０２による連鎖判定の処理フローを図７に示す。

ステップ７０１にて、ステップ５０３で求められた３次元または２次元の基線ベクトルあるいは相対距離と基準となった移動体の移動体ＩＤを受け取ると、関係性判定手段１２２のステップ５０２にて取得されている全ての移動体ＩＤに対応した各移動体の位置・姿勢および各移動体が計測した相対距離や対象移動体の特徴量を移動体情報記憶手段１２７から取得し（ステップ７０２）、この内、位置・姿勢の算出が行われた移動体を三次元空間上にマッピングする（ステップ７０３）。

次に、３次元または２次元の基線ベクトルあるいは相対距離などの他移動体の相対位置を計測した各移動体について、計測した対象の移動体を関連付けてゆく処理を繰り返す（ステップ７０４）。そこで、移動体情報記憶手段１２７に登録されている移動体の内、未処理の移動体の中から１つを処理対象として選択する（ステップ７０５）。

次に、ステップ７０６にて、相対位置計測の対象となった移動体の特徴が抽出可能かを判断する。ここで、移動体の特徴が抽出可能な場合としては、レーザー反射板などを被計測移動体が持っており、その反射をレーザーレンジファインダで計測した場合や、それぞれの移動体が特徴的な模様のあるマークを持っており、それをステレオカメラで認識できた場合などがある。もし、特徴が抽出可能であったなら、ステップ７０７に移行し、事前に決められた各移動体の特徴と比較して相対位置を計測した移動体を特定し、処理対象として選択した移動体とこの特定された移動体は繋がっていると判定する。

また、特徴が抽出不可能（相対位置の計測対象となった移動体が無い場合、即ち他の移動体が計測されなかった場合も含む）であるなら、ステップ７０８に移行し、計測された相対位置の誤差範囲内に存在している移動体を移動体情報記憶手段１２７の中から抽出する。この時、処理対象として選択した移動体で位置が算出されていない場合には、誤差範囲内に存在している移動体は無かったものとする。次にステップ７０９に移り、相対距離の誤差範囲内の移動体が複数あるかを判断する。もし抽出された移動体が一つの場合あるいは一つも無い場合は、ステップ７１０に移り、抽出された移動体が一つの場合にはその相対位置を計測された移動体を特定し、処理対象として選択した移動体とこの特定された移動体は繋がっていると判定する。なお、抽出された移動体が一つも無い場合には処理対象として選択した移動体についての処理を終了する。

抽出された移動体が複数ある場合は、ステップ７１１に移行し、計測した相対位置の情報が２次元以上の基線ベクトルの情報であるかを判断する。２次元以上の基線ベクトルを持つ場合は、ステップ７１２にて相対距離情報の水平方向の傾きに最も近い位置にある移動体を相対位置が計測された移動体として特定し、処理対象として選択した移動体とこの特定された移動体は繋がっていると判定する。もし、相対位置の情報が１次元、つまり相対距離のみの情報であるならば、基準となる移動体からの方向が特定されないため、ステップ７１３に移り、特定できなかった相対距離情報を持つ移動体の移動体ＩＤを記憶する。そして、ステップ７１４にて、全ての移動体に対して上記判定を行ったことを確認し、行っていない場合はステップ７０５からの処理を繰り返す。

全ての移動体に対してステップ７０５〜７１４のループ処理が行われたと判断された場合、ステップ７１５に移り、繋がっていると判定された移動体の組を抽出する。次に、ステップ７１６に移行し、ステップ７１５にて抽出された組の一方にでも共通する移動体の組を一つの集団としてまとめ、全ての移動体をグルーピングする。次にステップ７１７にて、ステップ７１６でグルーピングされた結果を移動体分類記憶手段１２３へ格納する。

この移動体をグルーピングする処理の概念を説明する。移動体Ａから、移動体Ｂの相対距離が計測できたとする。また同様に、移動体Ｂからは移動体Ｃと移動体Ｄ８０４の相対距離が計測できたとし、移動体Ｅからは移動体Ｆとの相対距離が計測できたとする。この時、移動体Ａ，移動体Ｂ，移動体Ｃ，移動体Ｄの間では直接的もしくは間接的にそれぞれの相対距離が計測されており、繋がっていると判断して、一つの集団と判定する。また、同様に移動体Ｅと移動体Ｆは直接相対距離が計測されているので繋がっていると判断し、一つの集団と判定する。しかし、移動体Ａを含む集団と移動体Ｅを含む集団の移動体間では、直接的にも間接的にも相対距離が計測されていないため、別の集団として判定されることになる。

再び図５のフローに戻り、ステップ５０５にて、信号独立性判定手段４０３では、同じ集団に属する各移動体の信号受信手段１０２にて得られたセンサ等の信号の内の測位信号が独立した発信源からの情報であるかどうかを判定する。ただし、発信源である測位衛星から発信される信号は唯一無二であるとする。信号独立性判定手段４０３の処理フローを図８に示す。

この処理では、連鎖判定手段４０２によりグルーピングされた各集団毎に、全ての集団について処理を行う（ステップ９００）。各集団において、まずステップ９０１にて、三角測量をするために必要な測位信号を受信した全ての移動体について、その移動体ＩＤとその移動体ＩＤを持つ各移動体から受信したセンサ情報の内、測位信号を読み出す。次にステップ９０２にて、読み出したそれらの測位信号に独立性判定ができるような符号が載っているかどうかを判断する。この独立性判定ができるような符号とは、ＧＰＳではＰＮコードのような識別符号を示す。ＰＮコードの分別には特許文献４に記載の手法などを用いる。このような発信源を特定する符号が測位信号にあった場合、直接発信源の独立性が判断できるため、ステップ９０３に移り、受信した測位信号に載っている符号が重複しないように、即ち発信源が重複し内容に測位信号を選ぶ。

独立性判定ができるような符号が受信した測位信号に載っていない場合、ステップ９０４に移行し、各移動体から受信したセンサ情報について観測行列を求める。例えば、求めたい位置の変数（ｘ，ｙ，ｚ）を未知変数として、（式１）で表されるような各測位信号を受信した場合の観測行列は、（式１）をある点（ｘ０，ｙ０，ｚ０）周りで１次の項までテーラー展開することで、以下の（式２）のように表すことができる。

（式２）のＧを観測行列と呼ぶ。取得したセンサ信号の数が行数であり、求めたい未知変数の数が列数となる。Ｇの要素は事前に取得し得る全ての行数分用意されているとする。Ｇは取得したセンサ信号に対応する要素を残し、その他の要素に０を代入することでＧを作成することができる。次にステップ９０５に移り、観測行列の階数（ランク）を求める。観測行列の階数は、ステップ９０４にて求めた観測行列Ｇを特異値分解もしくはＱＲ分解することで求めることができる。ここで求めた階数が独立な信号の数となる。

そして、ステップ９０６にて、重複している信号を消去する。同一の発信源をもつ測位信号が存在している場合、つまり、未知変数の数が観測行列Ｇの階数より大きかった場合、同一の発信源による測位信号を割り出し、それぞれが互いに独立した発信源による測位信号のみを残す。例えば、観測行列Ｇの要素において、１番目の測位信号による発信源までの距離ｒ１からｎ番目の測位信号による発信源までの距離信号ｒｎまで一つずつ、その測位信号に関わる要素を全て消去して、行列の階数を計算し、階数がｎと変わらない測位信号が重複する発信源による測位信号であると判断できることから、観測行列Ｇからその重複する発信源による測位信号に対応する行を全て消去することで重複する発信源による測位信号が無い観測行列Ｇができる。

また、ステップ９０７にて未知変数の数、つまり求めたい位置の要素数が独立な発信源の数より小さければ、位置補正が可能であるとして、ステップ９０８にて独立な発信源による測位信号を持つ移動体のＩＤとその測位信号を出力する。ここで、未知変数の数は上記の求めたい位置の変数の数となり、独立な発信源による測位信号の数はステップ９０６を経て、最終的に求めたＧの列数である。またもし未知変数の数が独立な発信源による測位信号の数より大きければ位置補正算出は不可能であるとして、ステップ９０９にてこの結果を出力する。

これらの処理を全てのグルーピングされた集団について繰り返す（ステップ９１０）ことで、各集団に属する移動体について位置補正情報を算出可能か否か、測位信号の独立性が判定される。

このようにして図５に示した処理フローにより判定された移動体間の関係性の結果として、図３に示した処理フローのステップ３０５により、これまでに求めた各移動体が属する集団や独立な測位信号および基線ベクトルを、移動体ＩＤと共に移動体分類記憶手段１２３に記憶する。

このようにしてセンタ１２１における移動体分類記憶手段１２３には、関係性判定手段１２２で判定された集団毎に、移動体から送信されてきた各移動体の位置・姿勢、他の移動体との相対位置情報、センサ情報のうち、独立した発信源による測位信号であると判定された測位信号分とその測位信号に対応した変数を記憶する。

センタ１２１の位置補正手段１２４は、移動体分類記憶手段１２３に記憶された情報から、各移動体の位置を算出し、もともと記憶されていた移動体の位置との差分を補正値とし、その誤差見積もりと共に補正情報として算出し、移動体位置補正情報記憶手段１２６に記憶する。ここで位置算出と誤差見積もりに用いる変数は、全て移動体分類記憶手段１２３にて記憶されていた変数とする。この算出の例として、移動体−１および移動体−２の２つの移動体の位置算出時の例を説明する。移動体−１および移動体−２の位置をそれぞれ、（ｘ１，ｙ１，ｚ１），（ｘ２，ｙ２，ｚ２）とする。移動体−１が、第一の信号発信源から第一の信号発信源と移動体−１間の距離情報ｒ１と、第二の信号発信源から第二の信号発信源と移動体−１間の距離情報ｒ２を受信したとする。また、同時に移動体−２が、第三の信号発信源から第三の信号発信源と移動体−２間の距離情報ｒ３と、第四の信号発信源から第四の信号発信源と移動体−２間の距離情報ｒ４を受信したとする。また、移動体−２では移動体−１までの相対位置を計測できたとして、移動体−２と移動体−１の間の相対距離の値がＬであったとして、移動体−１と移動体−２の基線ベクトルが（ｄｘ，ｄｙ，ｄｚ）であったとする。この時、移動体−１の位置（ｘ１，ｙ１，ｚ１）および移動体−２の位置（ｘ２，ｙ２，ｚ２）を求めるには、次の（式３）で表される連立方程式を解けばよい。

また、（式３）から算出された位置の誤差の見積もりは、ある点（ｘ０，ｙ０，ｚ０）を中心として、（式３）を１次の項までテーラー展開することで、（式４）のように表すことができる。

（式４）のＧを観測行列と呼び、この観測行列Ｇを用いることで観測値の誤差の分散σは、σ＝(ＧＧ^T)^-1のように観測された距離等の誤差の分散として算出される。

この（式４）で表された観測行列Ｇや上記の誤差の分散σをカルマンフィルタなどの確率モデルフィルタに適用することで、（式３）から算出された位置の誤差の見積もりが求められる。

センタ１２１の通信手段１２５は位置補正手段１２４で計算され移動体位置補正情報記憶手段に記憶された補正情報を移動体へ配信する。センタ１２１が通信手段１２５から送信する補正情報は、全ての求められた移動体の位置の補正情報に対してその補正をする対象の移動体の移動体ＩＤが割り振られており、配信を受信できた各移動体では、センタ１２１から配信された補正情報に割り振られた移動体ＩＤから、位置補正に使用する補正情報を特定し、その補正情報を基に、各移動体で位置を算出しなおす。

誤差分散情報は、移動体が位置補正情報として補正値と共にセンタから受け取り、移動体内部での位置推定時に用いることができる。これは例えば特許文献３に記載されているように、移動体の存在確率の計算に必要となる。

第一の実施例における位置推定システムでは、関係性判定手段をセンタ１２１に設けていたが、関係性判定手段を各移動体が持っている位置推定システムを第二の実施例として説明する。第二の実施例では、各移動体で関係性判定手段を備えていることから、位置補正を移動体でローカルに行うことができるため、大都市部など様々な車両が行き来し、かつ、センタとの通信も途切れる可能性がある場合に位置精度を向上させることができる。

第二の実施例の位置推定システムは、第一の実施例と同様、移動体の専用装置もしくは移動体に組み込まれることにより実現される。第一の実施例との違いは、複数の移動体で構成され、センタを必要としないことである。図９に、移動体１１０１における位置推定システムの構成を示す。移動体１１０１では、測位信号などのセンサ情報を取得する信号受信手段１１０２と、このセンサ情報を基に位置を算出する位置姿勢算出手段１１０３と、他の移動体と相対距離を計測する距離計測手段１１０４と、移動体のセンサ情報や算出した位置の情報を通信する通信手段１１０５と、位置補正情報を算出する位置補正手段１１０６と、移動体１１０１自身で得た、または他の移動体から得られた相対距離の計測の可否から他の移動体との繋がり等を判定する関係性判定手段１１０７と、位置補正手段１１０６にて算出した位置補正情報を記憶する位置補正情報記憶領域および、取得したセンサ情報や計測結果を記憶する計測情報記憶領域および、送信途中の値を記憶する中断情報記憶領域および、移動体の集団を記憶する移動体分類記憶領域および、他の移動体の情報を記憶する移動体情報記憶領域を設けた内部記憶手段１１０８と、距離計測手段１１０４にて計測した移動体の特徴を認識する特徴認識手段１１０９から構成される。

この内、信号受信手段１１０２と位置姿勢算出手段１１０３と距離計測手段１１０４と特徴認識手段１１０９は、それぞれ第一の実施例における信号受信手段１０２，位置姿勢算出手段１０３，距離計測手段１０４，特徴認識手段１０７と同様であるため説明を省略する。

また各移動体の信号受信手段１１０２は共にＧＰＳの測位信号を受信する機能を備えており、それぞれの移動体で管理する時刻はＧＰＳ時刻により同期されているものとする。

次に図１０に移動体における位置推定システムの処理フローを示す。以下の説明では移動体１１０１における動作として処理の説明を行う。まず、移動体１１０１では、所定の周期でセンサ等から得られる測位信号などのセンサ情報を信号受信手段１１０２にて取得する。また、特徴認識手段１１０９は、移動体１１０１の周囲を捜査し、第一の実施例の場合と同様にして他の移動体の特徴を検索し、移動体１１０１に対する方向と共に認識し、特徴量を求める。そして、認識した特徴までの相対距離を距離計測手段１１０４にて計測する（ステップ１２０１）。

次に、ステップ１２０２にて、ステップ１２０１で取得したセンサ情報および計測した相対距離や認識した特徴量を計測情報として内部記憶手段１１０８の計測情報記憶領域に記憶する。このとき、計測情報記憶領域に前回に記憶した情報が残っていても上書きされる。

次にステップ１２０３にて、位置補正情報が内部記憶手段１１０８の位置補正情報記憶領域にあるかどうかを確認し、もしあれば、ステップ１２０４に移行し、この位置補正情報を用いて現在の位置を補正する。もし位置補正情報が無ければ、ステップ１２０５に移行する。ステップ１２０５では、ステップ１２０２で記憶した計測情報を用いて位置・姿勢の算出が可能かどうかを判断する。もし可能であればステップ１２０６にて位置・姿勢を算出する。不可能であればステップ１２０７に移行する。

ステップ１２０７では他の移動体との通信が可能かどうかを判断する。他の移動体との通信可否の判断は、通信手段１１０５より応答要求のみの信号を出力し、ある一定時間内に他の移動体から応答が有った場合に通信可能と判断し、応答が無い場合には通信不可能と判断する。また、他の移動体から応答要求の通信があった場合には、このステップで要求元への応答を返す。他の移動体との通信が不可能であった場合、ステップ１２１８へ移行して、次の計測情報取得まで待機する。

他の移動体との通信が可能であれば、ステップ１２０８にて、内部記憶手段１１０８の中断情報記憶領域に前回の処理の途中で保留した情報があるかどうかを判断する。無い場合、ステップ１２０９にて、ステップ１２０２で記憶したセンサ等の情報および自移動体の移動体ＩＤ、他移動体を認識していれば認識した他移動体との相対距離や特徴量およびそれらを取得した時刻を他の移動体へ送信する。時刻は、前述のようにＧＰＳ時刻を用いる。

ステップ１２１０では、他の移動体がステップ１２０９で送った、センサ等の情報および移動体ＩＤ、認識した他の移動体との相対距離や特徴量およびそれらを取得した時刻を受信する。次に、ステップ１２１１にて、他の移動体から受信したセンサ情報およびその時刻、周囲で認識された移動体との相対距離とその特徴量などの情報を内部記憶手段１１０８における移動体情報記憶領域に記憶する。また、この時点である一定時間以上経った他の移動体の情報は削除する。また、同一の移動体ＩＤを持つ情報が存在する場合は、その情報を上書きする。

次にステップ１２１２にて、他の移動体からの情報が内部記憶手段１１０８の移動体情報記憶領域に記憶されているかどうかを判断する。他の移動体からの情報が記憶されていない場合、つまり他の移動体からの情報が一定時間の間に受信ができていないか、または受信した情報に他移動体の位置情報および特徴量の両方が揃っていない場合は、ステップ１２１６へ移り、内部記憶手段１１０８における位置補正情報記憶領域に、“位置補正情報無し”と記憶する。

もし、他の移動体からの情報が記憶されている場合はステップ１２１３に移り、関係性判定手段１１０７により情報を受信した移動体との関係性を判定し、関連があると判断した移動体について、受信した情報に含まれる測位信号の内、独立な発信源による測位信号の数と位置の補正に必要な未知変数の数を求める。そして求めた独立な発信源による測位信号の数と位置の補正に必要な未知変数の数を比べ、独立な発信源による測位信号の数が移動体の位置補正のために必要な未知変数の数よりも少ない場合、位置補正情報の算出が不可能と判定し、また、求めた独立な発信源による測位信号の数が移動体の位置補正のために必要な未知変数の数より多いかもしくは同じ数の場合、位置補正情報の算出は可能と判定する。

次にステップ１２１４にて、ステップ１２１３における判定結果から位置補正情報が算出可能かどうかを判断し、不可能であればステップ１２１６へ移り、内部記憶手段１１０８の位置補正情報記憶領域に、“位置補正情報無し”と記憶する。もし、位置補正情報が算出可能であれば、ステップ１２１５へ移行し、位置補正情報を算出する。ステップ１２１５にて位置補正情報が算出されれば、ステップ１２１６にて内部記憶手段１１０８の位置補正情報記憶領域に位置補正情報を記憶する。そして、ステップ１２１８で次の計測情報取得まで待機し、ステップ１２０１からの処理を繰り返すことになる。

このようにして保存された位置補正情報を元に、次の処理の際にステップ１２０４の処理で位置を補正するため、測位精度が向上する。ただし、ステップ１２０７以降は、次の計測情報取得の機会が来ると、割り込みがかかっていつでもその時点の処理を保留し、残った処理の情報を内部記憶手段１１０８の中断情報記憶領域に記憶してステップ１２０１に戻る。そして、再度ステップ１２０８に処理が移ってきた時に、内部記憶手段１１０８の中断情報記憶領域に保留情報が有ると、ステップ１２１７にて最後に処理を保留した所から処理を再開し、内部記憶手段１１０８の中断情報記憶領域に記憶された情報の処理に戻る。他の移動体の位置推定システムも上記の処理と同様に動作する。

次に各手段について説明する。移動体１１０１における信号受信手段１１０２，位置姿勢算出手段１１０３，距離計測手段１１０４，特徴認識手段１１０９は前述のように、第一の実施例における信号受信手段１０２，位置姿勢算出手段１０３，距離計測手段１０４，特徴認識手段１０７と同様である。

また、内部記憶手段１１０８は第一の実施例の内部記憶手段１０８に対応し、更に、第一の実施例におけるセンタに設けられていた移動体分類記憶手段１２３と移動体位置補正情報記憶手段１２６，移動体情報記憶手段１２７に対応して、それぞれ内部記憶手段１１０８に移動体分類記憶領域、位置補正情報記憶領域、移動体情報記憶領域が設けられており、以下の関係性判定手段１１０７の説明は、第一の実施例における関係性判定手段１２２及びこれに関連した連鎖判定手段４０２，信号独立性判定手段４０３，基線ベクトル算出手段４０４の動作の説明において、移動体分類記憶手段１２３，移動体位置補正情報記憶手段１２６，移動体情報記憶手段１２７がそれぞれ移動体分類記憶領域，位置補正情報記憶領域，移動体情報記憶領域に置き換えられたものと同様になる。

移動体１１０１における通信手段１１０５では、信号受信手段１１０２や位置姿勢算出手段１１０３や距離計測手段１１０４で得られた情報を他の移動体に送信する。また、他の移動体から送られてくる情報も受け取ることができる。

移動体１１０１における位置補正情報記憶領域では、前述のステップ１２１５で求めた位置補正情報を記憶する。ここで記憶された情報は、ステップ１２０４で位置の補正が行われた後に消去される。

移動体１１０１における関係性判定手段１１０７では、複数の移動体の関係性を判定することで、関連のある移動体を一まとめの集団として分類する。

関係性判定手段１１０７の構成は第一の実施例における関係性判定手段１２２と同様であり、その処理の流れも図５に示した処理と同様である。ただし、連鎖判定手段における処理では、ステップ７１７において、ステップ７１６でグルーピングされた結果を内部記憶手段１１０８における移動体分類記憶領域へ格納する点が第一の実施例と異なる。

また、ステップ１２１３における関係性判定手段１１０７の処理の際に算出された各移動体のグループや独立な信号および基線ベクトルは、内部記憶手段１１０８における移動体分類記憶領域に記憶される。そして、内部記憶手段１１０８における移動体分類記憶領域は、関係性判定手段１１０７にて判定された集団毎に、移動体より送信された位置姿勢、距離情報、センサ情報のうち、独立した信号であると判定された信号分とその信号を表す未知変数を記憶する。

位置補正手段１１０６は内部記憶手段１１０８における移動体分類記憶領域に記憶された情報から、各移動体の位置を算出し、もともと記憶されていた位置との差分を補正値として、その誤差見積もりと共に算出する。この時の算出方法は第一の実施例と同様である。

次に具体的な例を、図１１を使って説明する。この例では、移動体として車両を想定し、信号受信手段１１０２にＧＰＳの受信時刻が判るＧＰＳ受信機を搭載し、発信源をＧＰＳ衛星、測位信号をＧＰＳ信号としている。今、第一の車両１３０１において、第一のＧＰＳ衛星１３０２および第二のＧＰＳ衛星１３０３からのＧＰＳ信号がある時刻ｈ１で捕捉されたとする。また，同様にＧＰＳの受信時刻が判るＧＰＳ受信機を搭載した第二の車両１３１１では、第三のＧＰＳ衛星１３１２および第四のＧＰＳ衛星１３１３からのＧＰＳ信号がある時刻ｈ２で捕捉されたとする。また、第二の車両１３１１ではステレオカメラによりその光軸１３１４とある相対角度がずれた線１３１５上の距離Ｌの場所に、第一の車両１３０１を観測し、そのナンバープレートの車体番号を認識したとする。第一のＧＰＳ衛星１３０３から第一の車両１３０１までの距離情報ｒ１と第二のＧＰＳ衛星１３０３から第一の車両１３０１までの距離情報ｒ２を受信したとする。また、第二の車両１３１１が、第三のＧＰＳ衛星１３１２から第二の車両１３１１までの距離情報ｒ３と第四のＧＰＳ衛星１３１３から第二の車両１３１１までの距離情報ｒ４を受信したとする。

この状態のままでは、第一の車両１３０１と第二の車両１３１１はそれぞれ単独で位置を推定することができない。そこで、通信手段１１０５による車車間通信により第一の車両１３０１と第二の車両１３１１がそれぞれのＧＰＳ受信時刻を含むＧＰＳ受信情報および認識した車体番号を互いに通信する。そして、第一の車両１３０１および第二の車両の１３１１の関係性判定手段１１０７が起動され、まず、それぞれの基線ベクトル算出手段によって、第二の車両１３１１がステレオカメラにより計測した相対距離から基線ベクトルを算出される。

次に、第一の車両１３０１の関係性判定手段では第二の車両１３１１から特徴量として受信した認識対象車両の車体番号と第一の車両１３０１がデータとして持つ車体番号が一致することを確認し、第二の車両１３１１は同じグループに属していると認識する。同様に、第二の車両１３１１の関係性判定手段はステレオカメラで計測した車両の車体番号と、第一の車両１３０１の車体番号が一致していることを確認し、第一の車両１３０１が同じグループに属する車両であると認識する。次にそれぞれの車両で受信したＧＰＳ信号のＰＮコードから、第一のＧＰＳ衛星１３０２から第四のＧＰＳ衛星１３１３までは全て独立した衛星であると判断できる。これらの結果から、以下の（式５）の連立方程式を解くことでそれぞれの車両の位置が求まり、この値を位置補正情報として記憶する。

ここで、θはステレオカメラの光軸との水平方向のずれ角、ψはステレオカメラの光軸との垂直方向のずれ角、ｃは光速を表すものとする。

このように、単体ではＧＰＳ衛星が足りずに測位ができない場合でも、本装置を用いることで位置を算出できるようになり、測位精度が増すことができるようになる。

１０１，１１１，１１０１移動体
１０２，１１０２信号受信手段
１０３，１１０３位置姿勢算出手段
１０４，１１０４距離計測手段
１０５，１２５，１１０５通信手段
１０６位置補正情報記憶手段
１０７，１１０９特徴認識手段
１０８，１１０８内部記憶手段
１２１センタ
１２２，１１０７関係性判定手段
１２３移動体分類記憶手段
１２４，１１０６位置補正手段
１２６移動体位置補正情報記憶手段
１２７移動体情報記憶手段
４０２連鎖判定手段
４０３信号独立性判定手段
４０４基線ベクトル算出手段

Claims

複数の移動体からなり、前記移動体は信号受信手段により複数の発信源からの測位信号を受信して、自移動体の現在位置をその誤差見積もりと共に算出する測位手段と、
自移動体の周囲に存在する他の移動体との距離を計測する距離計測手段
を備えた位置推定システムにおいて、
前記移動体は
前記距離計測手段により距離を計測する他の移動体の特徴を認識する特徴認識手段と、
他の移動体もしくはセンタへ、受信した測位信号および算出した位置情報を送信し、位置補正情報を受信する通信手段と、
受信した位置補正情報を記憶する位置補正情報記憶手段と
を有し、
前記距離計測手段により、前記特徴認識手段で認識された他の移動体との間の距離を直接計測しているか、もしくは他の移動体から直接距離を計測されている移動体同士を同じ集団に分類する連鎖判定手段、
前記信号受信手段により受信した測位信号および前記連鎖判定手段において同じ集団に分類された各移動体で受信された測位信号から、測位信号の発信源が独立な測位信号を判別して選択する信号独立性判定手段、および
移動体の位置情報を用いて、前記距離計測手段により計測した他の移動体との距離に基づく相対位置情報から基線ベクトルを算出する基線ベクトル算出手段
を有する関係性判定手段と、
自移動体で受信した測位信号と前記関係性判定手段において選択した他の移動体で受信された測位信号に基づき現在位置を算出して位置補正情報を計算し、該位置補正情報を元に推定位置を補正する位置補正手段と、
を複数の移動体の内の少なくとも１台もしくは前記センタに備えた
ことを特徴とする位置推定システム。
請求項１に記載の位置推定システムにおいて、
前記基線ベクトル算出手段は、前記信号独立性判定手段により選択した独立な測位信号の数が、測位に必要な数以上の場合に、前記基線ベクトルを算出して移動体の位置を計算することを特徴とする位置推定システム。
請求項１に記載の位置推定システムにおいて、
前記信号独立性判定手段は、前記連鎖判定手段にて一つの集団と判断された移動体の集団において、集団全体での未知変数が、集団全体で未知変数以上の独立な発信源による測位信号の観測があることを判定することを特徴とする位置推定システム。