測位システム及び移動体
【課題】ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を算出することができるようにする。
【解決手段】通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する伝播時間算出部４−１を設け、相対位置・相対速度・相対時刻算出部５−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出する。
【解決手段】通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する伝播時間算出部４−１を設け、相対位置・相対速度・相対時刻算出部５−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムと、他の移動体との相対位置、相対速度及び相対時刻を算出する移動体とに関するものである。
【背景技術】
【０００２】
複数の移動体が共同作業を実施する場合（例えば、協調した測量等を実施する場合）、それぞれの移動体が自己位置を正確に測位する必要がある。
ただし、移動体が自己位置を測位する際には、移動体間の相対位置を正確に知る必要があり、絶対位置は相対位置ほど、精度を要求されないことがある。
【０００３】
移動体の位置を計測する測位装置（測位システム）としては、位置が既知である複数の電波源（例えば、ＧＰＳ衛星）から送信される電波（放射時刻が既知の電波）を受信し、その電波の送信時刻と受信時刻の差から移動体の位置を推定するものがある。
また、自律航法センサ（慣性航法センサ）により計測された加速度ベクトルを利用して、移動体の速度及び位置を計算するものがある。
一般的には、ＧＰＳ衛星から送信される電波を受信して測位を実施し、ＧＰＳ衛星から送信される電波を受信することができない場合に、自律航法センサにより計測された加速度ベクトルを利用するものが多い。
ただし、自律航法センサにより計測された加速度ベクトルを利用する場合、時間の経過に伴って測位結果の誤差が累積的に増加することが知られている。
【０００４】
【特許文献１】特開平１０−１７０２８９号公報（段落番号［００１３］から［００１９］、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従来の測位システムは以上のように構成されているので、ＧＰＳ衛星から送信される電波を受信することができない場合でも、移動体の位置を測位することができるが、自律航法センサにより計測された加速度ベクトルを長時間に亘って利用する場合、時間の経過に伴って測位結果の誤差が累積的に増加して、正確な測位結果が得られなくなることがあるなどの課題があった。
【０００６】
この発明は上記のような課題を解決するためになされたもので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を算出することができる測位システム及び移動体を得ることを目的とする。
【課題を解決するための手段】
【０００７】
この発明に係る測位システムは、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、他の移動体から送信時刻毎に送信された加速度ベクトルを受信する通信手段と、通信手段による加速度ベクトルの受信時刻と送信時刻間の時刻差を算出する時刻差算出手段とを設け、相対諸元算出手段が自律航法センサにより計測された加速度ベクトル、通信手段により受信された加速度ベクトル及び時刻差算出手段により算出された時刻差から、他の移動体との相対位置、相対速度及び相対時刻を算出するようにしたものである。
【発明の効果】
【０００８】
この発明によれば、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、他の移動体から送信時刻毎に送信された加速度ベクトルを受信する通信手段と、通信手段による加速度ベクトルの受信時刻と送信時刻間の時刻差を算出する時刻差算出手段とを設け、相対諸元算出手段が自律航法センサにより計測された加速度ベクトル、通信手段により受信された加速度ベクトル及び時刻差算出手段により算出された時刻差から、他の移動体との相対位置、相対速度及び相対時刻を算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果がある。
【発明を実施するための最良の形態】
【０００９】
実施の形態１．
図１はこの発明の実施の形態１による測位システムを示す構成図である。
図１では、測位システムがＭ台の移動体１−１〜１−Ｍから構成されており、移動体１−１〜１−Ｍが、他の移動体との相対位置、相対速度及び相対時刻を算出する例を示している。
図１において、自律航法センサ２−１〜２−Ｍは自己の移動体１−１〜１−Ｍの加速度ベクトルを計測する処理を実施する。
【００１０】
通信部３−１〜３−Ｍは例えば無線機などを搭載しており、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトルを他の移動体に送信し（例えば、移動体１−１→移動体１−Ｍ、移動体１−Ｍ→移動体１−１）、また、他の移動体から送信時刻ｔｎ毎に送信された加速度ベクトルを受信する処理を実施する。なお、通信部３−１〜３−Ｍは通信手段を構成している。
【００１１】
伝播時間算出部４−１〜４−Ｍは通信部３−１〜３−Ｍによる加速度ベクトルの受信時刻と、他の移動体に搭載されている通信部による加速度ベクトルの送信時刻ｔｎ間の時刻差を示す伝播時間を算出する処理を実施する。なお、伝播時間算出部４−１〜４−Ｍは時刻差算出手段を構成している。
相対位置・相対速度・相対時刻算出部５−１〜５−Ｍは自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル、通信部３−１〜３−Ｍにより受信された加速度ベクトル及び伝播時間算出部４−１〜４−Ｍにより算出された伝播時間から、他の移動体との相対位置、相対速度及び相対時刻を算出する処理を実施する。なお、相対位置・相対速度・相対時刻算出部５−１〜５−Ｍは相対諸元算出手段を構成している。
【００１２】
次に動作について説明する。
この実施の形態１では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルを移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルを受信することにより、移動体１−１の相対位置・相対速度・相対時刻算出部５−１が移動体１−２との相対位置、相対速度及び相対時刻を算出する例を説明する。
【００１３】
移動体１−１，１−２は、他の移動体１−２，１−１との相対位置等を算出する処理を開始する前の事前準備として、通信部３−１〜３−Ｍによる複数回（Ｎ回）の加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）を決定する。
なお、加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）は、事前に決定されていてもよいし、測定開始前に一方の移動体で送信時刻ｔｎを決定して、その送信時刻ｔｎを他の移動体に送信するようにしてもよい。
【００１４】
移動体１−１，１−２の自律航法センサ２−１，２−２は、自己の移動体１−１，１−Ｍの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を計測し、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を通信部３−１，３−２に出力するとともに、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を相対位置・相対速度・相対時刻算出部５−１，５−２に出力する。
【００１５】
移動体１−１の通信部３−１は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を所定の送信周波数（キャリア周波数）を用いて、その加速度ベクトルＡｓ（ｔｎ）に係る通信電波を移動体１−２に送信する。
また、移動体１−２の通信部３−２は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を所定の送信周波数（キャリア周波数）を用いて、その加速度ベクトルＡｒ（ｔｎ）に係る通信電波を移動体１−１に送信する。
【００１６】
ここでは、通信部３−１，３−２が送信時刻ｔｎになると、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するものについて示したが、その送信時刻ｔｎでは、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波の代わりに、例えば、加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）などのデータを含まない通信電波を送信し、後で、まとめて、全ての送信時刻ｔｎの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するようにしてもよい。
【００１７】
移動体１−１の通信部３−１は、移動体１−２の通信部３−２から送信された加速度ベクトルＡｒ（ｔｎ）に係る通信電波を受信すると、その通信電波から加速度ベクトルＡｒ（ｔｎ）を復調し、その加速度ベクトルＡｒ（ｔｎ）を伝播時間算出部４−１に出力する。
移動体１−１の伝播時間算出部４−１は、通信部３−１から加速度ベクトルＡｒ（ｔｎ）を受けると、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と、送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する。
この伝播時間τｃ（ｔｎ）には、移動体１−２から実際に伝播に要した時間の他に、２つの移動体１−１，１−２における内部クロックの時刻ずれ（相対時刻）が反映された値である。
伝播時間算出部４−１は、伝播時間τｃ（ｔｎ）を算出すると、その伝播時間τｃ（ｔｎ）と通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を相対位置・相対速度・相対時刻算出部５−１に出力する。
【００１８】
移動体１−１の相対位置・相対速度・相対時刻算出部５−１は、伝播時間算出部４−１から伝播時間τｃ（ｔｎ）と加速度ベクトルＡｒ（ｔｎ）を受け、自律航法センサ２−１から加速度ベクトルＡｓ（ｔｎ）を受けると、その加速度ベクトルＡｓ（ｔｎ）、加速度ベクトルＡｒ（ｔｎ）及び伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置、相対速度及び相対時刻を算出する。
以下、相対位置・相対速度・相対時刻算出部５−１の処理内容を具体的に説明する。
【００１９】
ここでは、送信時刻ｔ１における移動体１−１を基準とする相対位置をδｐ、相対速度をδｖでベクトル表現するものとする。
δｐ＝（δｐｘ，δｐｙ，δｐｚ）
δｖ＝（δｖｘ，δｖｙ，δｖｚ）
このベクトル表現は、移動体１−１，１−２が３次元空間を移動する場合であるが、移動体１−１，１−２が２次元平面内、もしくは、１次元直線内で移動する場合には、２次元もしくは１次元で表現すればよい。
【００２０】
次に、移動体１−１の内部クロックの時刻に対する移動体１−２の内部クロックの時刻のずれ（相対時刻）をδτ、光速をＣ、ユークリッド距離を‖・‖で表すものとすると、送信時刻ｔ１における伝播時間τｃ（ｔ１）に関して、下記の式（１）が成立する。
τｃ（ｔ１）・Ｃ＝‖δｐ‖−δτ・Ｃ （１）
送信時刻ｔ２における伝播時間τｃ（ｔ２）に関しては、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）が計測されているため、例えば、近似的に下記の式（２）が成立する。
τｃ（ｔ２）・Ｃ
＝‖（Ａｒ（ｔ１）−Ａｓ（ｔ１））（ｔ２−ｔ１）２／２＋（ｔ２−ｔ１）＋δｐ‖
−δτ・Ｃ
（２）
式（２）では、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）を用いて計算しているが、送信時刻ｔ１と送信時刻ｔ２の平均の加速度ベクトルを用いて計算してもよい。
【００２１】
同様にして、送信時刻ｔｎ（３≦ｎ≦Ｎ）における伝播時間τｃ（ｔｎ）に関する方程式をたてることができる。
相対位置・相対速度・相対時刻算出部５−１が算出する対象は、式（１）における相対位置δｐ、相対速度δｖ及び相対時刻δτであるので、移動体１−１，１−２が３次元空間を移動する場合には、合計７（＝３×２＋１）個の推定すべきパラメータがある。
この場合、加速度ベクトルの計測（通信）を７回実施すれば、推定すべきパラメータの数と方程式の数が同数となり、各パラメータを算出することができる。
ただし、実際には、計測誤差等が含まれるため、算出対象のパラメータの数と同数、もしくは、同数以上の計測（通信）を実施して、最小二乗解を求めることにより、各パラメータを算出することになる。
【００２２】
上記の方程式は非線形であるので、最小二乗解の求め方としては、非線形最小二乗法を用いるのが一般的である。
非線形最小二乗法としては、例えば、Ｎｅｗｔｏｎ法、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法、修正Ｍａｒｑｕａｒｄｔ法、ハイブリッド法など、多種のアルゴリズムが知られている。
相対位置・相対速度・相対時刻算出部５−１は、上記のようにして、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出すると、その相対位置δｐ、相対速度δｖ及び相対時刻δτを出力する。
【００２３】
以上で明らかなように、この実施の形態１によれば、加速度ベクトルＡｓ（ｔｎ）を計測する自律航法センサ２−１と、事前に決定された送信時刻毎ｔｎに自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を他の移動体２−２に送信する一方、他の移動体２−２から送信時刻ｔｎ毎に送信された加速度ベクトルＡｒ（ｔｎ）を受信する通信部３−１と、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する伝播時間算出部４−１とを設け、相対位置・相対速度・相対時刻算出部５−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果を奏する。
即ち、ＧＰＳ衛星等のように正確な位置や時刻が分る電波源を必要とせずに、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出することができる。また、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を利用するが、複数回の計測によって、その都度、相対位置δｐ・相対速度δｖを求めることが可能であり、誤差が累積してしまうことがない。
【００２４】
実施の形態２．
図２はこの発明の実施の形態２による測位システムを示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍは通信部３−１〜３−Ｍによる加速度ベクトルの受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出するとともに、通信部３−１〜３−Ｍによる加速度ベクトルの受信周波数と送信周波数間の周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する処理を実施する。なお、伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍは時刻差・周波数差算出手段を構成している。
【００２５】
相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍは自律航法センサ２−１により計測された加速度ベクトル、通信部３−１により受信された加速度ベクトル及び伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍにより算出された伝播時間・ドップラ周波数から、他の移動体との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する処理を実施する。なお、相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍは相対諸元算出手段を構成している。
【００２６】
上記実施の形態１では、移動体１−１〜１−Ｍの相対位置・相対速度・相対時刻算出部５−１〜５−Ｍが、他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するものについて示したが、移動体１−１〜１−Ｍの相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍが、他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出する他に、相対周波数δｆを算出するようにしてもよい。
具体的には、以下の通りである。
【００２７】
この実施の形態２では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度・相対時刻・相対周波数算出部１２−１が移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する例を説明する。
【００２８】
移動体１−１，１−２は、上記実施の形態１と同様に、他の移動体１−２，１−１との相対位置等を算出する処理を開始する前の事前準備として、通信部３−１〜３−Ｍによる複数回（Ｎ回）の加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）を決定する。
また、通信部３−１，３−２が加速度ベクトルを送信する際のキャリア周波数である送信周波数ｆＬを決定する。
【００２９】
移動体１−１，１−２の自律航法センサ２−１，２−２は、上記実施の形態１と同様に、自己の移動体１−１，１−Ｍの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を計測し、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を通信部３−１，３−２に出力するとともに、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を相対位置・相対速度・相対時刻・相対周波数算出部１２−１，１２−Ｍに出力する。
【００３０】
移動体１−１の通信部３−１は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を事前に決定された送信周波数ｆＬを用いて、その加速度ベクトルＡｓ（ｔｎ）に係る通信電波を移動体１−２に送信する。
また、移動体１−２の通信部３−２は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を事前に決定された送信周波数ｆＬを用いて、その加速度ベクトルＡｒ（ｔｎ）に係る通信電波を移動体１−１に送信する。
【００３１】
ここでは、通信部３−１，３−２が送信時刻ｔｎになると、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するものについて示したが、その送信時刻ｔｎでは、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波の代わりに、例えば、加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）などのデータを含まない通信電波を送信し、後で、まとめて、全ての送信時刻ｔｎの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するようにしてもよい。
【００３２】
移動体１−１の通信部３−１は、移動体１−２の通信部３−２から送信された加速度ベクトルＡｒ（ｔｎ）に係る通信電波を受信すると、その通信電波から加速度ベクトルＡｒ（ｔｎ）を復調し、その加速度ベクトルＡｒ（ｔｎ）を伝播時間・ドップラ周波数算出部１１−１に出力する。
移動体１−１の伝播時間・ドップラ周波数算出部１１−１は、通信部３−１から加速度ベクトルＡｒ（ｔｎ）を受けると、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と、送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する。
この伝播時間τｃ（ｔｎ）には、移動体１−２から実際に伝播に要した時間の他に、２つの移動体１−１，１−２における内部クロックの時刻ずれ（相対時刻）が反映された値である。
【００３３】
また、伝播時間・ドップラ周波数算出部１１−１は、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信周波数と、事前に決定された送信周波数ｆＬ間の周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する。
伝播時間・ドップラ周波数算出部１１−１は、伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出すると、その伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）と通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を相対位置・相対速度・相対時刻・相対周波数算出部１２−１に出力する。
【００３４】
移動体１−１の相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、伝播時間・ドップラ周波数算出部１１−１から伝播時間τｃ（ｔｎ）、ドップラ周波数Ｄｃ（ｔｎ）及び加速度ベクトルＡｒ（ｔｎ）を受け、自律航法センサ２−１から加速度ベクトルＡｓ（ｔｎ）を受けると、その伝播時間τｃ（ｔｎ）、ドップラ周波数Ｄｃ（ｔｎ）及び加速度ベクトルＡｒ（ｔｎ），Ａｓ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する。
以下、相対位置・相対速度・相対時刻・相対周波数算出部１２−１の処理内容を具体的に説明する。
【００３５】
相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、図１の相対位置・相対速度・相対時刻算出部５−１と同様にして、伝播時間に関する方程式をたてることができる。
さらに、相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、ドップラ周波数に関する方程式をたてることができる。
【００３６】
移動体１−１の送信周波数ｆＬ対する移動体１−２の送信周波数ｆＬのずれ（２つの移動体の基準周波数のずれ）をδｆで表すと、送信時刻ｔ１におけるドップラ周波数Ｄｃ（ｔ１）に関して、下記の式（３）が成立する。
【数１】
【００３７】
送信時刻ｔ２におけるドップラ周波数Ｄｃ（ｔ２）に関しては、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）が計測されているため、例えば、近似的に下記の式（４）が成立する。
【数２】
式（４）では、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）を用いて計算しているが、送信時刻ｔ１と送信時刻ｔ２の平均の加速度ベクトルを用いて計算してもよい。
【００３８】
同様にして、送信時刻ｔｎ（３≦ｎ≦Ｎ）におけるドップラ周波数Ｄｃ（ｔｎ）に関する方程式をたてることができる。
相対位置・相対速度・相対時刻・相対周波数算出部１２−１が算出する対象は、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆであるので、移動体１−１，１−２が３次元空間を移動する場合には、合計８（＝３×２＋１＋１）個の推定すべきパラメータ（上記実施の形態１で示したパラメータに、さらに相対周波数を加えたパラメータ）があるが、１回の加速度ベクトルの計測（通信）で伝播時間とドップラ周波数に関して２つの方程式が得られる。
このため、加速度ベクトルの計測（通信）を４回実施すれば、推定すべきパラメータの数と方程式の数が同数となり、各パラメータを算出することができる。
ただし、実際には、計測誤差等が含まれるため、算出対象のパラメータの数と同数、もしくは、同数以上の計測（通信）を実施して、最小二乗解を求めることにより、各パラメータを算出することになる。
最小二乗解の求め方は、上記実施の形態１と同様に、非線形最小二乗法を用いるのが一般的である。
【００３９】
事前に送信周波数ｆＬに関する校正が厳密に終了している場合には、送信周波数ｆＬのずれはないと想定することができるが、この場合は、δｆを変数ではなく、既知の値として非線形最小二乗法を解くことにより、推定すべきパラメータ数を減らすことができる。
相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、上記のようにして、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数τｆを算出すると、その相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数τｆを出力する。
【００４０】
以上で明らかなように、この実施の形態２によれば、加速度ベクトルＡｓ（ｔｎ）を計測する自律航法センサ２−１と、事前に決定された送信時刻ｔｎ毎に自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を他の移動体２−２に送信する一方、他の移動体２−２から送信時刻ｔｎ毎に送信された加速度ベクトルＡｒ（ｔｎ）を受信する通信部３−１と、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出するとともに、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信周波数と送信周波数間ｆＬの周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する伝播時間・ドップラ周波数算出部１１−１とを設け、相対位置・相対速度・相対時刻・相対周波数算出部１２−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間・ドップラ周波数算出部１１−１により算出された伝播時間τｃ（ｔｎ）・ドップラ周波数Ｄｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果を奏する。また、移動体１−１，１−２の間で基準周波数にずれがあっても、その基準周波数の差分を求めることができる効果を奏する。
また、上記実施の形態１よりも、少ない加速度ベクトルの計測（通信）回数で、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出することができる効果を奏する。
【００４１】
実施の形態３．
図３はこの発明の実施の形態３による測位システムを示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
差分値算出部２１−１〜２１−Ｍは伝播時間算出部４−１〜４−Ｍが伝播時間τｃ（ｔｎ）を算出する毎に、その伝播時間の差分値（例えば、τｃ（ｔｎ）−τｃ（ｔｎ−１））を算出する処理を実施する。
相対位置・相対速度算出部２２−１〜２２−Ｍは差分値算出部２１−１〜２１−Ｍにより算出された差分値、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対位置δｐ及び相対速度δｖを算出する処理を実施する。
【００４２】
相対時刻算出部２３−１〜２３−Ｍは相対位置・相対速度算出部２２−１〜２２−Ｍにより算出された他の移動体との相対位置δｐ及び相対速度δｖ、伝播時間算出部４−１〜４−Ｍにより算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対時刻δτを算出する処理を実施する。
なお、差分値算出部２１−１〜２１−Ｍ、相対位置・相対速度算出部２２−１〜２２−Ｍ及び相対時刻算出部２３−１〜２３−Ｍから相対諸元算出手段が構成されている。
【００４３】
上記実施の形態１では、相対位置・相対速度・相対時刻算出部５−１〜５−Ｍが他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するものについて示したが、相対位置・相対速度算出部２２−１〜２２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻算出部２３−１〜２３−Ｍが他の移動体との相対時刻δτを算出するようにしてもよい。
具体的には、以下の通りである。
【００４４】
この実施の形態３では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度算出部２２−１〜２２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出して、相対時刻算出部２３−１が移動体１−２との相対時刻δτを算出する例を説明する。
【００４５】
移動体１−１の差分値算出部２１−１は、伝播時間算出部４−１が上記実施の形態１と同様にして、伝播時間τｃ（ｔｎ）を算出する毎に、その伝播時間の差分値を算出する。
伝播時間の差分値の取り方としては、例えば、τｃ（ｔｎ）−τｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔｎ）−τｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔ２ｎ）−τｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
【００４６】
移動体１−１の相対位置・相対速度算出部２２−１は、差分値算出部２１−１が伝播時間の差分値を算出すると、その伝播時間の差分値、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）及び通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）から、移動体１−２との相対位置δｐ及び相対速度δｖを算出する。
相対位置・相対速度算出部２２−１の算出方法を以下に説明する。
【００４７】
差分値算出部２１−１が伝播時間の差分値を算出すると、その差分の方程式から相対時刻δτを消去することができる。
例えば、上記の式（２）−式（１）を計算すると、相対時刻δτの項がなくなることが分る。
このため、差分の方程式が相対位置δｐ及び相対速度δｖのパラメータ数、即ち、移動体１−１，１−２が３次元空間を移動する場合には６回以上、２次元空間を移動する場合には４回以上、伝播時間の差分を求めることができれば、上記実施の形態１と同様に、非線形最小二乗法等に基づいて移動体１−２との相対位置δｐ及び相対速度δｖを算出することができる。
【００４８】
移動体１−１の相対時刻算出部２３−１は、相対位置・相対速度算出部２２−１が移動体１−２との相対位置δｐ及び相対速度δｖを算出すると、例えば、式（１）や式（２）等の伝播時間に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対時刻δτを算出する。
【００４９】
以上で明らかなように、この実施の形態３によれば、相対位置・相対速度算出部２２−１〜２２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出してから、相対時刻算出部２３−１が移動体１−２との相対時刻δτを算出するように構成したので、非線形最少二乗法等で求める際に、パラメータ数を１つ減らした状況で、パラメータ値を推定することができるようになり、パラメータの推定の容易化を図ることができる効果を奏する。
【００５０】
実施の形態４．
図４はこの発明の実施の形態４による測位システムを示す構成図であり、図において、図２と同一符号は同一または相当部分を示すので説明を省略する。
差分値算出部３１−１〜３１−Ｍは伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍが伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出する毎に、その伝播時間の差分値（例えば、τｃ（ｔｎ）−τｃ（ｔｎ−１））及びドップラ周波数の差分値（例えば、Ｄｃ（ｔｎ）−Ｄｃ（ｔｎ））を算出する処理を実施する。
相対位置・相対速度算出部３２−１〜３２−Ｍは差分値算出部３１−１〜３１−Ｍにより算出された伝播時間の差分値及びドップラ周波数の差分値、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対位置δｐ及び相対速度δｖを算出する処理を実施する。
【００５１】
相対時刻・相対周波数算出部３３−１〜３３−Ｍは相対位置・相対速度算出部３２−１〜３２−Ｍにより算出された他の移動体との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍにより算出された伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対時刻δτ及び相対周波数δｆを算出する処理を実施する。
なお、差分値算出部３１−１〜３１−Ｍ、相対位置・相対速度算出部３２−１〜３２−Ｍ及び相対時刻・相対周波数算出部３３−１〜３３−Ｍから相対諸元算出手段が構成されている。
【００５２】
上記実施の形態２では、相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍが他の移動体との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出するものについて示したが、相対位置・相対速度算出部３２−１〜３２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻・相対周波数算出部３３−１〜３３−Ｍが他の移動体との相対時刻δτ及び相対周波数δｆを算出するようにしてもよい。
具体的には、以下の通りである。
【００５３】
この実施の形態４では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度算出部３２−１が移動体１−２との相対位置δｐ及び相対速度δｖを算出して、相対時刻・相対周波数算出部３３−１が移動体１−２との相対時刻δτ及び相対周波数δｆを算出する例を説明する。
【００５４】
移動体１−１の差分値算出部３１−１は、伝播時間・ドップラ周波数算出部１１−１が上記実施の形態２と同様にして、伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出する毎に、その伝播時間の差分値及びドップラ周波数の差分値を算出する。
伝播時間の差分値の取り方としては、例えば、τｃ（ｔｎ）−τｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔｎ）−τｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔ２ｎ）−τｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
また、ドップラ周波数の差分値の取り方としては、例えば、Ｄｃ（ｔｎ）−Ｄｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、Ｄｃ（ｔｎ）−Ｄｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、Ｄｃ（ｔ２ｎ）−Ｄｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
【００５５】
移動体１−１の相対位置・相対速度算出部３２−１は、差分値算出部３１−１が伝播時間の差分値及びドップラ周波数の差分値を算出すると、その伝播時間の差分値、そのドップラ周波数の差分値、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）及び通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）から、移動体１−２との相対位置δｐ及び相対速度δｖを算出する。
相対位置・相対速度算出部２２−１の算出方法を以下に説明する。
【００５６】
差分値算出部３１−１が伝播時間の差分値を算出すると、その差分の方程式から相対時刻δτを消去することができる。また、ドップラ周波数の差分値を算出すると、その差分の方程式から相対周波数δｆを消去することができる。
このため、差分の方程式が相対位置δｐ及び相対速度δｖのパラメータ数、即ち、移動体１−１，１−２が３次元空間を移動する場合には３回以上、２次元空間を移動する場合には２回以上、伝播時間及びドップラ周波数の差分を求めることができれば、上記実施の形態２と同様に、非線形最小二乗法等に基づいて移動体１−２との相対位置δｐ及び相対速度δｖを算出することができる。
【００５７】
移動体１−１の相対時刻・相対周波数算出部３３−１は、相対位置・相対速度算出部３２−１〜３２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出すると、例えば、式（１）や式（２）等の伝播時間に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１により算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対時刻δτを算出する。
また、相対時刻・相対周波数算出部３３−１は、例えば、式（３）や式（４）等のドップラ周波数に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１により算出されたドップラ周波数Ｄｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対周波数δｆを算出する。
【００５８】
以上で明らかなように、この実施の形態４によれば、相対位置・相対速度算出部３２−１〜３２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻・相対周波数算出部３３−１〜３３−Ｍが他の移動体との相対時刻δτ及び相対周波数δｆを算出するように構成したので、非線形最少二乗法等で求める際に、パラメータ数を２つ減らした状況で、パラメータ値を推定することができるようになり、パラメータの推定の容易化を図ることができる効果を奏する。
【００５９】
なお、上記実施の形態１〜４では、移動体１−１〜１−Ｍが加速度ベクトルに係る通信電波を送受信することにより測位するものについて示したが、電波を用いた通信に限るものではなく、例えば、音波や光波などに用いて加速度ベクトルを送受信するようにしてもよい。
また、移動体１−１〜１−Ｍの位置を３次元で測位するものについて示したが、２次元の場合でも、高度方向は地上の表面に移動体が存在するなどの条件を用いて計算すれば可能である。
さらに、移動体１−１〜１−Ｍの測位に関する機能は、ソフトウェアプログラムに基づいてＣＰＵを動作させることにより実行することも可能である。
【図面の簡単な説明】
【００６０】
【図１】この発明の実施の形態１による測位システムを示す構成図である。
【図２】この発明の実施の形態２による測位システムを示す構成図である。
【図３】この発明の実施の形態３による測位システムを示す構成図である。
【図４】この発明の実施の形態４による測位システムを示す構成図である。
【符号の説明】
【００６１】
１−１〜１−Ｍ 移動体、２−１〜２−Ｍ 自律航法センサ、３−１〜３−Ｍ 通信部（通信手段）、４−１〜４−Ｍ 伝播時間算出部（時刻差算出手段）、５−１〜５−Ｍ 相対位置・相対速度・相対時刻算出部（相対諸元算出手段）、１１−１〜１１−Ｍ 伝播時間・ドップラ周波数算出部（時刻差・周波数差算出手段）、１２−１〜１２−Ｍ 相対位置・相対速度・相対時刻・相対周波数算出部（相対諸元算出手段）、２１−１〜２１−Ｍ 差分値算出部（相対諸元算出手段）、２２−１〜２２−Ｍ 相対位置・相対速度算出部（相対諸元算出手段）、２３−１〜２３−Ｍ 相対時刻算出部（相対諸元算出手段）、３１−１〜３１−Ｍ 差分値算出部（相対諸元算出手段）、３２−１〜３２−Ｍ 相対位置・相対速度算出部（相対諸元算出手段）、３３−１〜３３−Ｍ 相対時刻・相対周波数算出部（相対諸元算出手段）。
【技術分野】
【０００１】
この発明は、複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムと、他の移動体との相対位置、相対速度及び相対時刻を算出する移動体とに関するものである。
【背景技術】
【０００２】
複数の移動体が共同作業を実施する場合（例えば、協調した測量等を実施する場合）、それぞれの移動体が自己位置を正確に測位する必要がある。
ただし、移動体が自己位置を測位する際には、移動体間の相対位置を正確に知る必要があり、絶対位置は相対位置ほど、精度を要求されないことがある。
【０００３】
移動体の位置を計測する測位装置（測位システム）としては、位置が既知である複数の電波源（例えば、ＧＰＳ衛星）から送信される電波（放射時刻が既知の電波）を受信し、その電波の送信時刻と受信時刻の差から移動体の位置を推定するものがある。
また、自律航法センサ（慣性航法センサ）により計測された加速度ベクトルを利用して、移動体の速度及び位置を計算するものがある。
一般的には、ＧＰＳ衛星から送信される電波を受信して測位を実施し、ＧＰＳ衛星から送信される電波を受信することができない場合に、自律航法センサにより計測された加速度ベクトルを利用するものが多い。
ただし、自律航法センサにより計測された加速度ベクトルを利用する場合、時間の経過に伴って測位結果の誤差が累積的に増加することが知られている。
【０００４】
【特許文献１】特開平１０−１７０２８９号公報（段落番号［００１３］から［００１９］、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従来の測位システムは以上のように構成されているので、ＧＰＳ衛星から送信される電波を受信することができない場合でも、移動体の位置を測位することができるが、自律航法センサにより計測された加速度ベクトルを長時間に亘って利用する場合、時間の経過に伴って測位結果の誤差が累積的に増加して、正確な測位結果が得られなくなることがあるなどの課題があった。
【０００６】
この発明は上記のような課題を解決するためになされたもので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を算出することができる測位システム及び移動体を得ることを目的とする。
【課題を解決するための手段】
【０００７】
この発明に係る測位システムは、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、他の移動体から送信時刻毎に送信された加速度ベクトルを受信する通信手段と、通信手段による加速度ベクトルの受信時刻と送信時刻間の時刻差を算出する時刻差算出手段とを設け、相対諸元算出手段が自律航法センサにより計測された加速度ベクトル、通信手段により受信された加速度ベクトル及び時刻差算出手段により算出された時刻差から、他の移動体との相対位置、相対速度及び相対時刻を算出するようにしたものである。
【発明の効果】
【０００８】
この発明によれば、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、他の移動体から送信時刻毎に送信された加速度ベクトルを受信する通信手段と、通信手段による加速度ベクトルの受信時刻と送信時刻間の時刻差を算出する時刻差算出手段とを設け、相対諸元算出手段が自律航法センサにより計測された加速度ベクトル、通信手段により受信された加速度ベクトル及び時刻差算出手段により算出された時刻差から、他の移動体との相対位置、相対速度及び相対時刻を算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果がある。
【発明を実施するための最良の形態】
【０００９】
実施の形態１．
図１はこの発明の実施の形態１による測位システムを示す構成図である。
図１では、測位システムがＭ台の移動体１−１〜１−Ｍから構成されており、移動体１−１〜１−Ｍが、他の移動体との相対位置、相対速度及び相対時刻を算出する例を示している。
図１において、自律航法センサ２−１〜２−Ｍは自己の移動体１−１〜１−Ｍの加速度ベクトルを計測する処理を実施する。
【００１０】
通信部３−１〜３−Ｍは例えば無線機などを搭載しており、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトルを他の移動体に送信し（例えば、移動体１−１→移動体１−Ｍ、移動体１−Ｍ→移動体１−１）、また、他の移動体から送信時刻ｔｎ毎に送信された加速度ベクトルを受信する処理を実施する。なお、通信部３−１〜３−Ｍは通信手段を構成している。
【００１１】
伝播時間算出部４−１〜４−Ｍは通信部３−１〜３−Ｍによる加速度ベクトルの受信時刻と、他の移動体に搭載されている通信部による加速度ベクトルの送信時刻ｔｎ間の時刻差を示す伝播時間を算出する処理を実施する。なお、伝播時間算出部４−１〜４−Ｍは時刻差算出手段を構成している。
相対位置・相対速度・相対時刻算出部５−１〜５−Ｍは自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル、通信部３−１〜３−Ｍにより受信された加速度ベクトル及び伝播時間算出部４−１〜４−Ｍにより算出された伝播時間から、他の移動体との相対位置、相対速度及び相対時刻を算出する処理を実施する。なお、相対位置・相対速度・相対時刻算出部５−１〜５−Ｍは相対諸元算出手段を構成している。
【００１２】
次に動作について説明する。
この実施の形態１では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルを移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルを受信することにより、移動体１−１の相対位置・相対速度・相対時刻算出部５−１が移動体１−２との相対位置、相対速度及び相対時刻を算出する例を説明する。
【００１３】
移動体１−１，１−２は、他の移動体１−２，１−１との相対位置等を算出する処理を開始する前の事前準備として、通信部３−１〜３−Ｍによる複数回（Ｎ回）の加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）を決定する。
なお、加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）は、事前に決定されていてもよいし、測定開始前に一方の移動体で送信時刻ｔｎを決定して、その送信時刻ｔｎを他の移動体に送信するようにしてもよい。
【００１４】
移動体１−１，１−２の自律航法センサ２−１，２−２は、自己の移動体１−１，１−Ｍの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を計測し、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を通信部３−１，３−２に出力するとともに、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を相対位置・相対速度・相対時刻算出部５−１，５−２に出力する。
【００１５】
移動体１−１の通信部３−１は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を所定の送信周波数（キャリア周波数）を用いて、その加速度ベクトルＡｓ（ｔｎ）に係る通信電波を移動体１−２に送信する。
また、移動体１−２の通信部３−２は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を所定の送信周波数（キャリア周波数）を用いて、その加速度ベクトルＡｒ（ｔｎ）に係る通信電波を移動体１−１に送信する。
【００１６】
ここでは、通信部３−１，３−２が送信時刻ｔｎになると、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するものについて示したが、その送信時刻ｔｎでは、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波の代わりに、例えば、加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）などのデータを含まない通信電波を送信し、後で、まとめて、全ての送信時刻ｔｎの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するようにしてもよい。
【００１７】
移動体１−１の通信部３−１は、移動体１−２の通信部３−２から送信された加速度ベクトルＡｒ（ｔｎ）に係る通信電波を受信すると、その通信電波から加速度ベクトルＡｒ（ｔｎ）を復調し、その加速度ベクトルＡｒ（ｔｎ）を伝播時間算出部４−１に出力する。
移動体１−１の伝播時間算出部４−１は、通信部３−１から加速度ベクトルＡｒ（ｔｎ）を受けると、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と、送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する。
この伝播時間τｃ（ｔｎ）には、移動体１−２から実際に伝播に要した時間の他に、２つの移動体１−１，１−２における内部クロックの時刻ずれ（相対時刻）が反映された値である。
伝播時間算出部４−１は、伝播時間τｃ（ｔｎ）を算出すると、その伝播時間τｃ（ｔｎ）と通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を相対位置・相対速度・相対時刻算出部５−１に出力する。
【００１８】
移動体１−１の相対位置・相対速度・相対時刻算出部５−１は、伝播時間算出部４−１から伝播時間τｃ（ｔｎ）と加速度ベクトルＡｒ（ｔｎ）を受け、自律航法センサ２−１から加速度ベクトルＡｓ（ｔｎ）を受けると、その加速度ベクトルＡｓ（ｔｎ）、加速度ベクトルＡｒ（ｔｎ）及び伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置、相対速度及び相対時刻を算出する。
以下、相対位置・相対速度・相対時刻算出部５−１の処理内容を具体的に説明する。
【００１９】
ここでは、送信時刻ｔ１における移動体１−１を基準とする相対位置をδｐ、相対速度をδｖでベクトル表現するものとする。
δｐ＝（δｐｘ，δｐｙ，δｐｚ）
δｖ＝（δｖｘ，δｖｙ，δｖｚ）
このベクトル表現は、移動体１−１，１−２が３次元空間を移動する場合であるが、移動体１−１，１−２が２次元平面内、もしくは、１次元直線内で移動する場合には、２次元もしくは１次元で表現すればよい。
【００２０】
次に、移動体１−１の内部クロックの時刻に対する移動体１−２の内部クロックの時刻のずれ（相対時刻）をδτ、光速をＣ、ユークリッド距離を‖・‖で表すものとすると、送信時刻ｔ１における伝播時間τｃ（ｔ１）に関して、下記の式（１）が成立する。
τｃ（ｔ１）・Ｃ＝‖δｐ‖−δτ・Ｃ （１）
送信時刻ｔ２における伝播時間τｃ（ｔ２）に関しては、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）が計測されているため、例えば、近似的に下記の式（２）が成立する。
τｃ（ｔ２）・Ｃ
＝‖（Ａｒ（ｔ１）−Ａｓ（ｔ１））（ｔ２−ｔ１）２／２＋（ｔ２−ｔ１）＋δｐ‖
−δτ・Ｃ
（２）
式（２）では、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）を用いて計算しているが、送信時刻ｔ１と送信時刻ｔ２の平均の加速度ベクトルを用いて計算してもよい。
【００２１】
同様にして、送信時刻ｔｎ（３≦ｎ≦Ｎ）における伝播時間τｃ（ｔｎ）に関する方程式をたてることができる。
相対位置・相対速度・相対時刻算出部５−１が算出する対象は、式（１）における相対位置δｐ、相対速度δｖ及び相対時刻δτであるので、移動体１−１，１−２が３次元空間を移動する場合には、合計７（＝３×２＋１）個の推定すべきパラメータがある。
この場合、加速度ベクトルの計測（通信）を７回実施すれば、推定すべきパラメータの数と方程式の数が同数となり、各パラメータを算出することができる。
ただし、実際には、計測誤差等が含まれるため、算出対象のパラメータの数と同数、もしくは、同数以上の計測（通信）を実施して、最小二乗解を求めることにより、各パラメータを算出することになる。
【００２２】
上記の方程式は非線形であるので、最小二乗解の求め方としては、非線形最小二乗法を用いるのが一般的である。
非線形最小二乗法としては、例えば、Ｎｅｗｔｏｎ法、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法、修正Ｍａｒｑｕａｒｄｔ法、ハイブリッド法など、多種のアルゴリズムが知られている。
相対位置・相対速度・相対時刻算出部５−１は、上記のようにして、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出すると、その相対位置δｐ、相対速度δｖ及び相対時刻δτを出力する。
【００２３】
以上で明らかなように、この実施の形態１によれば、加速度ベクトルＡｓ（ｔｎ）を計測する自律航法センサ２−１と、事前に決定された送信時刻毎ｔｎに自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を他の移動体２−２に送信する一方、他の移動体２−２から送信時刻ｔｎ毎に送信された加速度ベクトルＡｒ（ｔｎ）を受信する通信部３−１と、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する伝播時間算出部４−１とを設け、相対位置・相対速度・相対時刻算出部５−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果を奏する。
即ち、ＧＰＳ衛星等のように正確な位置や時刻が分る電波源を必要とせずに、他の移動体１−２との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出することができる。また、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を利用するが、複数回の計測によって、その都度、相対位置δｐ・相対速度δｖを求めることが可能であり、誤差が累積してしまうことがない。
【００２４】
実施の形態２．
図２はこの発明の実施の形態２による測位システムを示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍは通信部３−１〜３−Ｍによる加速度ベクトルの受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出するとともに、通信部３−１〜３−Ｍによる加速度ベクトルの受信周波数と送信周波数間の周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する処理を実施する。なお、伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍは時刻差・周波数差算出手段を構成している。
【００２５】
相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍは自律航法センサ２−１により計測された加速度ベクトル、通信部３−１により受信された加速度ベクトル及び伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍにより算出された伝播時間・ドップラ周波数から、他の移動体との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する処理を実施する。なお、相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍは相対諸元算出手段を構成している。
【００２６】
上記実施の形態１では、移動体１−１〜１−Ｍの相対位置・相対速度・相対時刻算出部５−１〜５−Ｍが、他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するものについて示したが、移動体１−１〜１−Ｍの相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍが、他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出する他に、相対周波数δｆを算出するようにしてもよい。
具体的には、以下の通りである。
【００２７】
この実施の形態２では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度・相対時刻・相対周波数算出部１２−１が移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する例を説明する。
【００２８】
移動体１−１，１−２は、上記実施の形態１と同様に、他の移動体１−２，１−１との相対位置等を算出する処理を開始する前の事前準備として、通信部３−１〜３−Ｍによる複数回（Ｎ回）の加速度ベクトルの送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）を決定する。
また、通信部３−１，３−２が加速度ベクトルを送信する際のキャリア周波数である送信周波数ｆＬを決定する。
【００２９】
移動体１−１，１−２の自律航法センサ２−１，２−２は、上記実施の形態１と同様に、自己の移動体１−１，１−Ｍの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を計測し、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を通信部３−１，３−２に出力するとともに、その加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）を相対位置・相対速度・相対時刻・相対周波数算出部１２−１，１２−Ｍに出力する。
【００３０】
移動体１−１の通信部３−１は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を事前に決定された送信周波数ｆＬを用いて、その加速度ベクトルＡｓ（ｔｎ）に係る通信電波を移動体１−２に送信する。
また、移動体１−２の通信部３−２は、事前に決定された送信時刻ｔｎ（ｎ＝１，２，３，・・・，Ｎ）になると、自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を事前に決定された送信周波数ｆＬを用いて、その加速度ベクトルＡｒ（ｔｎ）に係る通信電波を移動体１−１に送信する。
【００３１】
ここでは、通信部３−１，３−２が送信時刻ｔｎになると、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するものについて示したが、その送信時刻ｔｎでは、自律航法センサ２−１，２−２により計測された加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波の代わりに、例えば、加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）などのデータを含まない通信電波を送信し、後で、まとめて、全ての送信時刻ｔｎの加速度ベクトルＡｓ（ｔｎ），Ａｒ（ｔｎ）に係る通信電波を他の移動体１−２，１−１に送信するようにしてもよい。
【００３２】
移動体１−１の通信部３−１は、移動体１−２の通信部３−２から送信された加速度ベクトルＡｒ（ｔｎ）に係る通信電波を受信すると、その通信電波から加速度ベクトルＡｒ（ｔｎ）を復調し、その加速度ベクトルＡｒ（ｔｎ）を伝播時間・ドップラ周波数算出部１１−１に出力する。
移動体１−１の伝播時間・ドップラ周波数算出部１１−１は、通信部３−１から加速度ベクトルＡｒ（ｔｎ）を受けると、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と、送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出する。
この伝播時間τｃ（ｔｎ）には、移動体１−２から実際に伝播に要した時間の他に、２つの移動体１−１，１−２における内部クロックの時刻ずれ（相対時刻）が反映された値である。
【００３３】
また、伝播時間・ドップラ周波数算出部１１−１は、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信周波数と、事前に決定された送信周波数ｆＬ間の周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する。
伝播時間・ドップラ周波数算出部１１−１は、伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出すると、その伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）と通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を相対位置・相対速度・相対時刻・相対周波数算出部１２−１に出力する。
【００３４】
移動体１−１の相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、伝播時間・ドップラ周波数算出部１１−１から伝播時間τｃ（ｔｎ）、ドップラ周波数Ｄｃ（ｔｎ）及び加速度ベクトルＡｒ（ｔｎ）を受け、自律航法センサ２−１から加速度ベクトルＡｓ（ｔｎ）を受けると、その伝播時間τｃ（ｔｎ）、ドップラ周波数Ｄｃ（ｔｎ）及び加速度ベクトルＡｒ（ｔｎ），Ａｓ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出する。
以下、相対位置・相対速度・相対時刻・相対周波数算出部１２−１の処理内容を具体的に説明する。
【００３５】
相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、図１の相対位置・相対速度・相対時刻算出部５−１と同様にして、伝播時間に関する方程式をたてることができる。
さらに、相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、ドップラ周波数に関する方程式をたてることができる。
【００３６】
移動体１−１の送信周波数ｆＬ対する移動体１−２の送信周波数ｆＬのずれ（２つの移動体の基準周波数のずれ）をδｆで表すと、送信時刻ｔ１におけるドップラ周波数Ｄｃ（ｔ１）に関して、下記の式（３）が成立する。
【数１】
【００３７】
送信時刻ｔ２におけるドップラ周波数Ｄｃ（ｔ２）に関しては、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）が計測されているため、例えば、近似的に下記の式（４）が成立する。
【数２】
式（４）では、送信時刻ｔ１における加速度ベクトルＡｓ（ｔ１），Ａｒ（ｔ１）を用いて計算しているが、送信時刻ｔ１と送信時刻ｔ２の平均の加速度ベクトルを用いて計算してもよい。
【００３８】
同様にして、送信時刻ｔｎ（３≦ｎ≦Ｎ）におけるドップラ周波数Ｄｃ（ｔｎ）に関する方程式をたてることができる。
相対位置・相対速度・相対時刻・相対周波数算出部１２−１が算出する対象は、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆであるので、移動体１−１，１−２が３次元空間を移動する場合には、合計８（＝３×２＋１＋１）個の推定すべきパラメータ（上記実施の形態１で示したパラメータに、さらに相対周波数を加えたパラメータ）があるが、１回の加速度ベクトルの計測（通信）で伝播時間とドップラ周波数に関して２つの方程式が得られる。
このため、加速度ベクトルの計測（通信）を４回実施すれば、推定すべきパラメータの数と方程式の数が同数となり、各パラメータを算出することができる。
ただし、実際には、計測誤差等が含まれるため、算出対象のパラメータの数と同数、もしくは、同数以上の計測（通信）を実施して、最小二乗解を求めることにより、各パラメータを算出することになる。
最小二乗解の求め方は、上記実施の形態１と同様に、非線形最小二乗法を用いるのが一般的である。
【００３９】
事前に送信周波数ｆＬに関する校正が厳密に終了している場合には、送信周波数ｆＬのずれはないと想定することができるが、この場合は、δｆを変数ではなく、既知の値として非線形最小二乗法を解くことにより、推定すべきパラメータ数を減らすことができる。
相対位置・相対速度・相対時刻・相対周波数算出部１２−１は、上記のようにして、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数τｆを算出すると、その相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数τｆを出力する。
【００４０】
以上で明らかなように、この実施の形態２によれば、加速度ベクトルＡｓ（ｔｎ）を計測する自律航法センサ２−１と、事前に決定された送信時刻ｔｎ毎に自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）を他の移動体２−２に送信する一方、他の移動体２−２から送信時刻ｔｎ毎に送信された加速度ベクトルＡｒ（ｔｎ）を受信する通信部３−１と、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信時刻と送信時刻ｔｎ間の時刻差を示す伝播時間τｃ（ｔｎ）を算出するとともに、通信部３−１による加速度ベクトルＡｒ（ｔｎ）の受信周波数と送信周波数間ｆＬの周波数差を示すドップラ周波数Ｄｃ（ｔｎ）を算出する伝播時間・ドップラ周波数算出部１１−１とを設け、相対位置・相対速度・相対時刻・相対周波数算出部１２−１が自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）及び伝播時間・ドップラ周波数算出部１１−１により算出された伝播時間τｃ（ｔｎ）・ドップラ周波数Ｄｃ（ｔｎ）から、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出するように構成したので、ＧＰＳ衛星から送信される電波を受信することなく、正確な測位結果を得ることができる効果を奏する。また、移動体１−１，１−２の間で基準周波数にずれがあっても、その基準周波数の差分を求めることができる効果を奏する。
また、上記実施の形態１よりも、少ない加速度ベクトルの計測（通信）回数で、他の移動体１−２との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出することができる効果を奏する。
【００４１】
実施の形態３．
図３はこの発明の実施の形態３による測位システムを示す構成図であり、図において、図１と同一符号は同一または相当部分を示すので説明を省略する。
差分値算出部２１−１〜２１−Ｍは伝播時間算出部４−１〜４−Ｍが伝播時間τｃ（ｔｎ）を算出する毎に、その伝播時間の差分値（例えば、τｃ（ｔｎ）−τｃ（ｔｎ−１））を算出する処理を実施する。
相対位置・相対速度算出部２２−１〜２２−Ｍは差分値算出部２１−１〜２１−Ｍにより算出された差分値、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対位置δｐ及び相対速度δｖを算出する処理を実施する。
【００４２】
相対時刻算出部２３−１〜２３−Ｍは相対位置・相対速度算出部２２−１〜２２−Ｍにより算出された他の移動体との相対位置δｐ及び相対速度δｖ、伝播時間算出部４−１〜４−Ｍにより算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対時刻δτを算出する処理を実施する。
なお、差分値算出部２１−１〜２１−Ｍ、相対位置・相対速度算出部２２−１〜２２−Ｍ及び相対時刻算出部２３−１〜２３−Ｍから相対諸元算出手段が構成されている。
【００４３】
上記実施の形態１では、相対位置・相対速度・相対時刻算出部５−１〜５−Ｍが他の移動体との相対位置δｐ、相対速度δｖ及び相対時刻δτを算出するものについて示したが、相対位置・相対速度算出部２２−１〜２２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻算出部２３−１〜２３−Ｍが他の移動体との相対時刻δτを算出するようにしてもよい。
具体的には、以下の通りである。
【００４４】
この実施の形態３では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度算出部２２−１〜２２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出して、相対時刻算出部２３−１が移動体１−２との相対時刻δτを算出する例を説明する。
【００４５】
移動体１−１の差分値算出部２１−１は、伝播時間算出部４−１が上記実施の形態１と同様にして、伝播時間τｃ（ｔｎ）を算出する毎に、その伝播時間の差分値を算出する。
伝播時間の差分値の取り方としては、例えば、τｃ（ｔｎ）−τｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔｎ）−τｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔ２ｎ）−τｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
【００４６】
移動体１−１の相対位置・相対速度算出部２２−１は、差分値算出部２１−１が伝播時間の差分値を算出すると、その伝播時間の差分値、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）及び通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）から、移動体１−２との相対位置δｐ及び相対速度δｖを算出する。
相対位置・相対速度算出部２２−１の算出方法を以下に説明する。
【００４７】
差分値算出部２１−１が伝播時間の差分値を算出すると、その差分の方程式から相対時刻δτを消去することができる。
例えば、上記の式（２）−式（１）を計算すると、相対時刻δτの項がなくなることが分る。
このため、差分の方程式が相対位置δｐ及び相対速度δｖのパラメータ数、即ち、移動体１−１，１−２が３次元空間を移動する場合には６回以上、２次元空間を移動する場合には４回以上、伝播時間の差分を求めることができれば、上記実施の形態１と同様に、非線形最小二乗法等に基づいて移動体１−２との相対位置δｐ及び相対速度δｖを算出することができる。
【００４８】
移動体１−１の相対時刻算出部２３−１は、相対位置・相対速度算出部２２−１が移動体１−２との相対位置δｐ及び相対速度δｖを算出すると、例えば、式（１）や式（２）等の伝播時間に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間算出部４−１により算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対時刻δτを算出する。
【００４９】
以上で明らかなように、この実施の形態３によれば、相対位置・相対速度算出部２２−１〜２２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出してから、相対時刻算出部２３−１が移動体１−２との相対時刻δτを算出するように構成したので、非線形最少二乗法等で求める際に、パラメータ数を１つ減らした状況で、パラメータ値を推定することができるようになり、パラメータの推定の容易化を図ることができる効果を奏する。
【００５０】
実施の形態４．
図４はこの発明の実施の形態４による測位システムを示す構成図であり、図において、図２と同一符号は同一または相当部分を示すので説明を省略する。
差分値算出部３１−１〜３１−Ｍは伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍが伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出する毎に、その伝播時間の差分値（例えば、τｃ（ｔｎ）−τｃ（ｔｎ−１））及びドップラ周波数の差分値（例えば、Ｄｃ（ｔｎ）−Ｄｃ（ｔｎ））を算出する処理を実施する。
相対位置・相対速度算出部３２−１〜３２−Ｍは差分値算出部３１−１〜３１−Ｍにより算出された伝播時間の差分値及びドップラ周波数の差分値、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対位置δｐ及び相対速度δｖを算出する処理を実施する。
【００５１】
相対時刻・相対周波数算出部３３−１〜３３−Ｍは相対位置・相対速度算出部３２−１〜３２−Ｍにより算出された他の移動体との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１〜１１−Ｍにより算出された伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）、自律航法センサ２−１〜２−Ｍにより計測された加速度ベクトル及び通信部３−１〜３−Ｍにより受信された加速度ベクトルから、他の移動体との相対時刻δτ及び相対周波数δｆを算出する処理を実施する。
なお、差分値算出部３１−１〜３１−Ｍ、相対位置・相対速度算出部３２−１〜３２−Ｍ及び相対時刻・相対周波数算出部３３−１〜３３−Ｍから相対諸元算出手段が構成されている。
【００５２】
上記実施の形態２では、相対位置・相対速度・相対時刻・相対周波数算出部１２−１〜１２−Ｍが他の移動体との相対位置δｐ、相対速度δｖ、相対時刻δτ及び相対周波数δｆを算出するものについて示したが、相対位置・相対速度算出部３２−１〜３２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻・相対周波数算出部３３−１〜３３−Ｍが他の移動体との相対時刻δτ及び相対周波数δｆを算出するようにしてもよい。
具体的には、以下の通りである。
【００５３】
この実施の形態４では、説明の便宜上、測位システムが２台（Ｍ＝２）の移動体１−１，１−２から構成されており、移動体１−１，１−２が他の移動体１−２，１−１の相対位置等を算出するものを想定する。
特に、移動体１−２の通信部３−２が自律航法センサ２−２により計測された加速度ベクトルＡｒ（ｔｎ）を移動体１−１に送信し、移動体１−１の通信部３−１が移動体１−２から送信された加速度ベクトルＡｒ（ｔｎ）を受信することにより、移動体１−１の相対位置・相対速度算出部３２−１が移動体１−２との相対位置δｐ及び相対速度δｖを算出して、相対時刻・相対周波数算出部３３−１が移動体１−２との相対時刻δτ及び相対周波数δｆを算出する例を説明する。
【００５４】
移動体１−１の差分値算出部３１−１は、伝播時間・ドップラ周波数算出部１１−１が上記実施の形態２と同様にして、伝播時間τｃ（ｔｎ）及びドップラ周波数Ｄｃ（ｔｎ）を算出する毎に、その伝播時間の差分値及びドップラ周波数の差分値を算出する。
伝播時間の差分値の取り方としては、例えば、τｃ（ｔｎ）−τｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔｎ）−τｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、τｃ（ｔ２ｎ）−τｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
また、ドップラ周波数の差分値の取り方としては、例えば、Ｄｃ（ｔｎ）−Ｄｃ（ｔ１）（２≦ｎ≦Ｎ）としてもよいし、Ｄｃ（ｔｎ）−Ｄｃ（ｔｎ−１）（２≦ｎ≦Ｎ）としてもよいし、Ｄｃ（ｔ２ｎ）−Ｄｃ（ｔ２ｎ−１）（１≦ｎ≦Ｎ／２）としてもよい。
【００５５】
移動体１−１の相対位置・相対速度算出部３２−１は、差分値算出部３１−１が伝播時間の差分値及びドップラ周波数の差分値を算出すると、その伝播時間の差分値、そのドップラ周波数の差分値、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）及び通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）から、移動体１−２との相対位置δｐ及び相対速度δｖを算出する。
相対位置・相対速度算出部２２−１の算出方法を以下に説明する。
【００５６】
差分値算出部３１−１が伝播時間の差分値を算出すると、その差分の方程式から相対時刻δτを消去することができる。また、ドップラ周波数の差分値を算出すると、その差分の方程式から相対周波数δｆを消去することができる。
このため、差分の方程式が相対位置δｐ及び相対速度δｖのパラメータ数、即ち、移動体１−１，１−２が３次元空間を移動する場合には３回以上、２次元空間を移動する場合には２回以上、伝播時間及びドップラ周波数の差分を求めることができれば、上記実施の形態２と同様に、非線形最小二乗法等に基づいて移動体１−２との相対位置δｐ及び相対速度δｖを算出することができる。
【００５７】
移動体１−１の相対時刻・相対周波数算出部３３−１は、相対位置・相対速度算出部３２−１〜３２−Ｍが移動体１−２との相対位置δｐ及び相対速度δｖを算出すると、例えば、式（１）や式（２）等の伝播時間に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１により算出された伝播時間τｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対時刻δτを算出する。
また、相対時刻・相対周波数算出部３３−１は、例えば、式（３）や式（４）等のドップラ周波数に関する方程式に、移動体１−２との相対位置δｐ及び相対速度δｖ、伝播時間・ドップラ周波数算出部１１−１により算出されたドップラ周波数Ｄｃ（ｔｎ）、自律航法センサ２−１により計測された加速度ベクトルＡｓ（ｔｎ）、通信部３−１により受信された加速度ベクトルＡｒ（ｔｎ）を代入することにより、移動体１−２との相対周波数δｆを算出する。
【００５８】
以上で明らかなように、この実施の形態４によれば、相対位置・相対速度算出部３２−１〜３２−Ｍが他の移動体との相対位置δｐ及び相対速度δｖを算出してから、相対時刻・相対周波数算出部３３−１〜３３−Ｍが他の移動体との相対時刻δτ及び相対周波数δｆを算出するように構成したので、非線形最少二乗法等で求める際に、パラメータ数を２つ減らした状況で、パラメータ値を推定することができるようになり、パラメータの推定の容易化を図ることができる効果を奏する。
【００５９】
なお、上記実施の形態１〜４では、移動体１−１〜１−Ｍが加速度ベクトルに係る通信電波を送受信することにより測位するものについて示したが、電波を用いた通信に限るものではなく、例えば、音波や光波などに用いて加速度ベクトルを送受信するようにしてもよい。
また、移動体１−１〜１−Ｍの位置を３次元で測位するものについて示したが、２次元の場合でも、高度方向は地上の表面に移動体が存在するなどの条件を用いて計算すれば可能である。
さらに、移動体１−１〜１−Ｍの測位に関する機能は、ソフトウェアプログラムに基づいてＣＰＵを動作させることにより実行することも可能である。
【図面の簡単な説明】
【００６０】
【図１】この発明の実施の形態１による測位システムを示す構成図である。
【図２】この発明の実施の形態２による測位システムを示す構成図である。
【図３】この発明の実施の形態３による測位システムを示す構成図である。
【図４】この発明の実施の形態４による測位システムを示す構成図である。
【符号の説明】
【００６１】
１−１〜１−Ｍ 移動体、２−１〜２−Ｍ 自律航法センサ、３−１〜３−Ｍ 通信部（通信手段）、４−１〜４−Ｍ 伝播時間算出部（時刻差算出手段）、５−１〜５−Ｍ 相対位置・相対速度・相対時刻算出部（相対諸元算出手段）、１１−１〜１１−Ｍ 伝播時間・ドップラ周波数算出部（時刻差・周波数差算出手段）、１２−１〜１２−Ｍ 相対位置・相対速度・相対時刻・相対周波数算出部（相対諸元算出手段）、２１−１〜２１−Ｍ 差分値算出部（相対諸元算出手段）、２２−１〜２２−Ｍ 相対位置・相対速度算出部（相対諸元算出手段）、２３−１〜２３−Ｍ 相対時刻算出部（相対諸元算出手段）、３１−１〜３１−Ｍ 差分値算出部（相対諸元算出手段）、３２−１〜３２−Ｍ 相対位置・相対速度算出部（相対諸元算出手段）、３３−１〜３３−Ｍ 相対時刻・相対周波数算出部（相対諸元算出手段）。
【特許請求の範囲】
【請求項１】
複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムにおいて、上記複数の移動体は、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に上記自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、上記他の移動体から上記送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出する時刻差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差算出手段により算出された時刻差から、上記他の移動体との相対位置、相対速度及び相対時刻を算出する相対諸元算出手段とを備えていることを特徴とする測位システム。
【請求項２】
複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムにおいて、上記複数の移動体は、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に上記自律航法センサにより計測された加速度ベクトルを事前に決定された送信周波数で他の移動体に送信する一方、上記他の移動体から上記送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出するとともに、上記通信手段による加速度ベクトルの受信周波数と上記送信周波数間の周波数差を算出する時刻差・周波数差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対位置、相対速度、相対時刻及び相対周波数を算出する相対諸元算出手段とを備えていることを特徴とする測位システム。
【請求項３】
相対諸元算出手段は、時刻差算出手段が時刻差を算出する毎に、その時刻差間の差分値を算出する差分値算出部と、上記差分値算出部により算出された差分値、自律航法センサにより計測された加速度ベクトル及び通信手段により受信された加速度ベクトルから、他の移動体との相対位置及び相対速度を算出する相対位置・相対速度算出部と、上記相対位置・相対速度算出部の算出結果と上記時刻差算出手段により算出された時刻差から、他の移動体との相対時刻を算出する相対時刻算出部とから構成されていることを特徴とする請求項１記載の測位システム。
【請求項４】
相対諸元算出手段は、時刻差・周波数差算出手段が時刻差及び周波数差を算出する毎に、その時刻差間の差分値及び周波数差間の差分値を算出する差分値算出部と、上記差分値算出部により算出された差分値、自律航法センサにより計測された加速度ベクトル及び通信手段により受信された加速度ベクトルから、他の移動体との相対位置及び相対速度を算出する相対位置・相対速度算出部と、上記相対位置・相対速度算出部の算出結果と上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対時刻及び相対周波数を算出する相対時刻・相対周波数算出部とから構成されていることを特徴とする請求項２記載の測位システム。
【請求項５】
通信手段は、事前に決定された送信時刻では、自律航法センサにより計測された加速度ベクトルを送信する代わりに、加速度ベクトルデータを含まない電波を他の移動体に送信し、後で、まとめて、全ての送信時刻の加速度ベクトルを他の移動体に送信することを特徴とする請求項１から請求項４のうちのいずれか１項記載の測位システム。
【請求項６】
加速度ベクトルを計測する自律航法センサと、他の移動体から事前に決定された送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出する時刻差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差算出手段により算出された時刻差から、上記他の移動体との相対位置、相対速度及び相対時刻を算出する相対諸元算出手段とを備えた移動体。
【請求項７】
加速度ベクトルを計測する自律航法センサと、他の移動体から事前に決定された送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出するとともに、上記通信手段による加速度ベクトルの受信周波数と自己の送信周波数間の周波数差を算出する時刻差・周波数差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対位置、相対速度、相対時刻及び相対周波数を算出する相対諸元算出手段とを備えた移動体。
【請求項１】
複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムにおいて、上記複数の移動体は、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に上記自律航法センサにより計測された加速度ベクトルを他の移動体に送信する一方、上記他の移動体から上記送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出する時刻差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差算出手段により算出された時刻差から、上記他の移動体との相対位置、相対速度及び相対時刻を算出する相対諸元算出手段とを備えていることを特徴とする測位システム。
【請求項２】
複数の移動体間の相対位置、相対速度及び相対時刻を算出する測位システムにおいて、上記複数の移動体は、加速度ベクトルを計測する自律航法センサと、事前に決定された送信時刻毎に上記自律航法センサにより計測された加速度ベクトルを事前に決定された送信周波数で他の移動体に送信する一方、上記他の移動体から上記送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出するとともに、上記通信手段による加速度ベクトルの受信周波数と上記送信周波数間の周波数差を算出する時刻差・周波数差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対位置、相対速度、相対時刻及び相対周波数を算出する相対諸元算出手段とを備えていることを特徴とする測位システム。
【請求項３】
相対諸元算出手段は、時刻差算出手段が時刻差を算出する毎に、その時刻差間の差分値を算出する差分値算出部と、上記差分値算出部により算出された差分値、自律航法センサにより計測された加速度ベクトル及び通信手段により受信された加速度ベクトルから、他の移動体との相対位置及び相対速度を算出する相対位置・相対速度算出部と、上記相対位置・相対速度算出部の算出結果と上記時刻差算出手段により算出された時刻差から、他の移動体との相対時刻を算出する相対時刻算出部とから構成されていることを特徴とする請求項１記載の測位システム。
【請求項４】
相対諸元算出手段は、時刻差・周波数差算出手段が時刻差及び周波数差を算出する毎に、その時刻差間の差分値及び周波数差間の差分値を算出する差分値算出部と、上記差分値算出部により算出された差分値、自律航法センサにより計測された加速度ベクトル及び通信手段により受信された加速度ベクトルから、他の移動体との相対位置及び相対速度を算出する相対位置・相対速度算出部と、上記相対位置・相対速度算出部の算出結果と上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対時刻及び相対周波数を算出する相対時刻・相対周波数算出部とから構成されていることを特徴とする請求項２記載の測位システム。
【請求項５】
通信手段は、事前に決定された送信時刻では、自律航法センサにより計測された加速度ベクトルを送信する代わりに、加速度ベクトルデータを含まない電波を他の移動体に送信し、後で、まとめて、全ての送信時刻の加速度ベクトルを他の移動体に送信することを特徴とする請求項１から請求項４のうちのいずれか１項記載の測位システム。
【請求項６】
加速度ベクトルを計測する自律航法センサと、他の移動体から事前に決定された送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出する時刻差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差算出手段により算出された時刻差から、上記他の移動体との相対位置、相対速度及び相対時刻を算出する相対諸元算出手段とを備えた移動体。
【請求項７】
加速度ベクトルを計測する自律航法センサと、他の移動体から事前に決定された送信時刻毎に送信された加速度ベクトルを受信する通信手段と、上記通信手段による加速度ベクトルの受信時刻と上記送信時刻間の時刻差を算出するとともに、上記通信手段による加速度ベクトルの受信周波数と自己の送信周波数間の周波数差を算出する時刻差・周波数差算出手段と、上記自律航法センサにより計測された加速度ベクトル、上記通信手段により受信された加速度ベクトル及び上記時刻差・周波数差算出手段の算出結果から、上記他の移動体との相対位置、相対速度、相対時刻及び相対周波数を算出する相対諸元算出手段とを備えた移動体。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００９−２０４５７９（Ｐ２００９−２０４５７９Ａ）
【公開日】平成２１年９月１０日（２００９．９．１０）
【国際特許分類】
【出願番号】特願２００８−４９９１６（Ｐ２００８−４９９１６）
【出願日】平成２０年２月２９日（２００８．２．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２１年９月１０日（２００９．９．１０）
【国際特許分類】
【出願日】平成２０年２月２９日（２００８．２．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]