JP2013061307A - 移動体の相対姿勢計測方法と装置 - Google Patents

Abstract

【課題】２つの移動体の相対姿勢を精度よく求めることができる手段を提供する。
【解決手段】第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ_１を計測する。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ_２を計測する。計測した方位θ_１と方位θ_２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係θ_ｔを、第１および第２の移動体の相対姿勢として求める。
【選択図】図１

Description

本発明は、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法と装置に関する。

複数の移動体が、互いに独立してエリア内を移動する場合に、複数の移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御などを行うことがある。
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。

上述の制御を行うために、移動体の位置と姿勢を、次の方法Ａ、ＢまたはＣにより計測している。

方法Ａでは、移動体に、速度センサやジャイロセンサなどを設置する。速度センサによる速度計測値を時間積分して、移動体の位置を求め、ジャイロセンサによる角速度計測値を時間積分して、移動体の姿勢を求める。

方法Ｂでは、移動体に、外界センサを搭載する。この外界センサにより、移動体の外部において既知の位置に存在する指標の方向と、該指標までの距離を計測する。この計測結果を、地図データと照合する。この照合に基づいて、移動体の自己位置と姿勢を求める。例えば、外界センサとして３次元レーザレーダやカメラを用いる。なお、下記の特許文献１では、宇宙機である移動体の姿勢として、当該移動体の進行方向と前記指標に相当する地球や太陽との相対関係を検出している。

方法Ｃでは、移動体の外部に設置された計測機器（例えば、カメラ）により移動体を含む画像を取得し、この画像に基づいて移動体の位置と姿勢を求める。

なお、後述する本発明の実施形態で使用可能な技術が、下記の特許文献２〜４に記載されている。

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

しかし、移動体の位置と姿勢のうち、姿勢を精度よく計測することは難しい。

上述した方法Ａでは、ジャイロセンサの計測誤差が積分されることにより、移動体の姿勢の計測精度が低下する可能性がある。

上述した方法Ｂでは、３次元レーザレーダやカメラにより指標を認識する必要がある。そのため、指標を設定できない場合には、移動体の姿勢を計測できない。

上述した方法Ｃでは、移動体の姿勢を得るためには、計測機器により、移動体における３つの位置を互いに区別して計測する必要がある。従って、移動体と計測機器との距離が大きくなると、移動体における３つの位置を互いに区別して計測することが困難となる。

一方、２つの移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御を行う場合には、２つの移動体の相対姿勢が得られればよい。すなわち、移動体が移動するエリアに固定された静止座標系から見た各移動体の姿勢を求めることは必ずしも必要ではない。

そこで、本発明の目的は、２つの移動体の相対姿勢を精度よく求めることができる方法と装置を提供することにある。

上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
（Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
（Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ_１を計測し、
（Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ_２を計測し、
（Ｄ）計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法が提供される。

本発明の好ましい実施形態によると、前記（Ｂ）を行う時の前記相対姿勢は、前記（Ｃ）を行う時の前記相対姿勢と同じであり、
前記（Ｂ）（Ｃ）で計測された方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との前記相対関係を幾何学的に算出する。

本発明の別の実施形態によると、前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ_１と方位θ_２に基づいて更新し、
更新された確率分布に基づいて、前記相対関係を推定する。

好ましくは、前記移動体の位置および姿勢を内部状態とし、
該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ_１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ_２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。

好ましい別の例では、前記（Ｄ）において、
第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
前記方位θ_１と方位θ_２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
前記抽出した各サンプルについて、前記方位θ_１と方位θ_２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
前記方位θ_１と方位θ_２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
当該抽出したサンプルに基づいて、前記相対関係を推定する。

また、上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ_１を計測し、
第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ_２を計測し、
計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置が提供される。

上述した本発明によると、第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ_１を計測し、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ_２を計測し、計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
この方法では、第１の方位センサは、第２の移動体の方位を認識できればよく、第２の移動体における３つの位置を互いに区別して計測する必要がない。第２の方位センサも同様である。
よって、第１の移動体と第２の移動体との距離が大きくても、相対姿勢を高精度に求めることができる。

本発明の実施形態による相対姿勢計測装置を示す。 （Ａ）は、第１の移動体に固定した２次元座標系を示し、（Ｂ）は、第２の移動体に固定した２次元座標系を示す。 本発明の実施形態による相対姿勢計測方法を示すフローチャートである。 第１の移動体に固定された３次元座標系Ａと、第２の移動体に固定された３次元座標系Ｂを示す。 （Ａ）は、第１の方位センサが計測する方位θ_１ｚ、θ_１ｘｙを示し、（Ｂ）は、第２の方位センサが計測する方位θ_２ｚ、θ_２ｘｙを示す。

本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明の実施形態による相対姿勢計測装置１０の構成図である。相対姿勢計測装置１０は、第１の移動体３に対する第２の移動体５の相対姿勢を計測する。
図１の例では、第１および第２の移動体３、５は、路面を走行するものであり、図１は、各移動体３、５を上方から見た平面図である。なお、図１において、各移動体３、５の位置を検出する位置検出装置７（３次元レーザレーダ）が、移動体３、５以外の固定位置に設けられている。

相対姿勢計測装置１０は、第１の方位センサ９と第２の方位センサ１１と演算部１３とを備える。

第１の方位センサ９は、第１の移動体３に設けられる。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ_１を計測する。ここで、第１基準方向Ｄ１とは、第１の移動体３に設定された基点Ｐ１から、該基点Ｐ１と間隔をおいて第１の移動体３に設定された方位点Ｐ２へ向かう方向である。好ましくは、第１基準方向Ｄ１は、第１の移動体３の前後方向であって、第１の移動体３の後方から前方へ向かう方向である。ただし、第１基準方向Ｄ１は、第１の移動体３の前後方向から既知の角度だけ傾いた方向であってもよい。

第１の方位センサ９は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第１基準方向Ｄ１を向くように、または、第１基準方向Ｄ１に対して既知の角度だけ傾いた方向を向くように第１の移動体３に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第２の移動体５との位置関係に基づいて、第２の移動体５が位置する方位θ_１を求める。この時、画像処理装置は、第２の移動体５に設けられた指標を、前記画像内において第２の移動体５の位置として特定してよい。なお、好ましくは、第１の移動体３から見てすべての方向の画像が得られるように、全方向の視野を有する１つのカメラを第１の移動体３に設置するか、もしくは、異なる方向を向く複数のカメラを第１の移動体３に設置するのがよい。

ここで、θ_１は、例えば、第１の移動体３に固定された２次元座標系Ａで表現されてよい。例えば、２次元座標系Ａは、図２（Ａ）に示すように、その原点が基点Ｐ１であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第１基準方向Ｄ１に相当する。すなわち、ｘ軸上に方位点Ｐ２が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ_１は、図２（Ａ）において、第１基準方向Ｄ１（ｘ軸）に対する角度であり、基点Ｐ１を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ１を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。

第２の方位センサ１１は、第２の移動体５に設けられる。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ_２を計測する。ここで、第２基準方向Ｄ２とは、第２の移動体５に設定された基点Ｐ３から、該基点Ｐ３と間隔をおいて第２の移動体５に設定された方位点Ｐ４へ向かう方向である。好ましくは、第２基準方向Ｄ２は、第２の移動体５の前後方向であって、第２の移動体５の後方から前方へ向かう方向である。ただし、第２基準方向Ｄ２は、第２の移動体５の前後方向から既知の角度だけ傾いた方向であってもよい。

第２の方位センサ１１は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第２基準方向Ｄ２を向くように、または、第２基準方向Ｄ２に対して既知の角度だけ傾いた方向を向くように第２の移動体５に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第１の移動体３との位置関係に基づいて、第１の移動体３が位置する方位θ_２を求める。この時、画像処理装置は、第１の移動体３に設けられた指標を、前記画像内において第１の移動体３の位置として特定してよい。なお、好ましくは、第２の移動体５から見てすべての方位の画像が得られるように、全方向の視野を有する１つのカメラを第２の移動体５に設置するか、もしくは、異なる方向を向く複数のカメラを第２の移動体５に設置するのがよい。

ここで、θ_２は、例えば、第２の移動体５に固定された２次元座標系Ｂで表現されてよい。例えば、２次元座標系Ｂは、図２（Ｂ）に示すように、その原点が基点Ｐ３であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第２基準方向Ｄ２に相当する。すなわち、ｘ軸上に方位点Ｐ４が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ_２は、図２（Ｂ）において、第２基準方向Ｄ２（ｘ軸）に対する角度であり、基点Ｐ３を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ３を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。

各移動体３、５に設ける上述の指標として、例えば、次のものを用いてよい。
一例では、指標は、特許文献２のように、特定のパターンで輝度が変化する発光ダイオードである。この場合、方位センサ９、１１は、カメラを用いて、短い設定時間にわたる動画を画像として撮像し、画像処理装置を用いて、画像において、特定のパターンで輝度が変化する画素の位置を、指標の位置として特定する。
別の例では、指標は、特許文献３、４に記載されたような幾何学的模様である。この場合、方位センサ９、１１の画像処理装置は、幾何学的模様と同じ模様を予め記憶しており、カメラを用いて得た画像において、予め記憶した当該模様と同じ模様の部分を、指標の位置として特定する。

上述の指標は、第１および第２の移動体３、５の外面（例えば、外周面）に、間隔をおいて取り付けられていてよい。

演算部１３は、第１の方位センサ９が計測した方位θ_１と、第１の方位センサ９が計測した方位θ_２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係を求める。求めた相対関係が、第１の移動体３に対する第２の移動体５の相対姿勢となる。相対関係は、例えば、図１のように、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度θ_ｔで表わされる。このなす角度θ_ｔは、第１基準方向Ｄ１、または、この方向Ｄ１に平行な方向と、第２基準方向Ｄ２、または、この方向Ｄ２に平行な方向とのなす角度を意味する。

図３は、上述した相対姿勢計測装置１０を用いた相対姿勢計測方法を示すフローチャートである。

ステップＳ１において、第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。
ステップＳ２において、第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ_１を計測する。
ステップＳ３において、第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ_２を計測する。
ステップＳ４において、演算部１３は、計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
なお、ステップＳ２、Ｓ３は、移動体３、５が移動している時に行われてよい。

以下、ステップＳ４で行われる演算部１３による演算方法１、２を詳しく説明する。

［演算方法１］
演算方法１は、ステップＳ２を行う時の前記相対姿勢が、ステップＳ３を行う時の前記相対姿勢と同じである場合に採用される。このような場合としては、ステップＳ２を行う時とステップＳ３を行う時との間で、第１および第２の移動体３、５の姿勢と位置が変化していない場合がある。演算方法１を行う他の場合として、ステップＳ２とステップＳ３を同じ時刻に行う場合がある。

演算部１３は、ステップＳ２とステップＳ３でそれぞれ計測した方位θ_１と方位θ_２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係（なす角度θ_ｔ）を幾何学的演算により算出する。ここでは、上述の図２のように、方位θ_１、θ_２は、上述の２次元座標系Ａ、Ｂで表現できるものとする。なお、図１において、Ｄ１ｐは、第１基準方向と平行であって基点Ｐ３を通る直線である。
演算部１３は、方位θ_１と方位θ_２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度をθ_ｔを、次の式（１）により求める。なお、この式（１）において、θ_１とθ_２とθ_ｔの単位はラジアンである。

θ_ｔ＝θ_１−θ_２−π ・・・（１）

上述では、第１および第２の移動体３、５が、２次元平面である路面を移動する場合について説明したので、ここでは、方位θ_１、θ_２は、２次元座標系Ａ、Ｂで表わした。しかし、第１および第２の移動体３、５が、３次元空間内を移動する場合には、演算部１３は、３次元座標系で表わされた方位θ_１、θ_２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度を次のように求める。

図４は、移動体３、５が移動する空間において、これらの移動体３、５にそれぞれ固定された３次元座標系Ａ、Ｂを示す。各３次元座標系Ａ，Ｂは、互いに直交するｘ軸とｙ軸とｚ軸を有する。上述の第１基準方向Ｄ１は、３次元座標系Ａのｘ軸に一致し、上述の第２基準方向Ｄ２は、３次元座標系Ｂのｘ軸に一致する。

図４において、第１の方位センサ９は、上述の方位θ_１として方位θ_１ｚとθ_１ｘｙを計測し、第２の方位センサ１１は、上述の方位θ_２として方位θ_２ｚとθ_２ｘｙを計測する。なお、図４において、Ｐ１_ｘｙは、３次元座標系Ａの原点Ｐ１を３次元座標系Ｂのｘ−ｙ平面に正射影した点であり、Ｐ３_ｘｙは、３次元座標系Ｂの原点Ｐ３を３次元座標系Ａのｘ−ｙ平面に正射影した点である。

また、第１基準方向Ｄ１をベクトルｖ_１で表わし、第１基準方向Ｄ２をベクトルｖ_２で表わし、Ｒ_ｔを回転行列として、

ｖ_１＝Ｒ_ｔｖ_２

が成り立つとする。この場合、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係は、上記のＲ_ｔで表わすことができる。ここで、Ｒ_ｔは、例えば、

Ｒ_ｔ＝Ｒ_１ｚ（π）Ｒ_１ｚ（-θ_１ｚ）Ｒ_１ｘｙ（θ_１ｘｙ）Ｒ_２ｘｙ（θ_２ｘｙ）Ｒ_２ｚ（θ_２ｚ）

で表わされる。
Ｒ_２ｚ（θ_２ｚ）は、空間上の各点を、３次元座標系Ｂのｚ軸回りに、θ_２ｚだけ回転させる行列である。Ｒ_２ｘｙ（θ_２ｘｙ）は、３次元座標系ＢをＲ_２ｚ（θ_２ｚ）により回転させた座標系Ｂ１のｙ軸回りに、空間上の各点を、θ_２ｘｙだけ回転させる行列である。Ｒ_１ｘｙ（θ_１ｘｙ）は、３次元座標系Ａのｚ軸回りに、座標系Ａをθ_１ｚだけ回転させた座標系Ａ１のｙ軸回りに、空間上の各点を、θ_１ｘｙだけ回転させる行列である。Ｒ_１ｚ（−θ_１ｚ）は、３次元座標系Ａのｚ軸回りに、空間上の各点を、−θ_１ｚだけ回転させる行列である。Ｒ_１ｚ（π）は、空間上の各点を、３次元座標系Ａのｚ軸回りに、π（ラジアン）だけ回転させる行列である。なお、各回転角θは、大きさを表しており、θは、各軸の正の方向から見て反時計回りの回転を示し、−θは、各軸の正の方向から見て時計回りの回転を示している。

［演算方法２］
演算方法２は、第１の方位センサ９と第２の方位センサ１１が、異なる時刻に、または、同じ時刻に、上述した方位θ１と方位θ２を計測する場合に採用される。
この場合、演算部１３は、ベイズフィルタ（ベイズの定理）を用いて、前記相対関係を推定する。すなわち、演算部１３は、前記移動体の位置および姿勢の確率分布を、前記方位θ_１と方位θ_２に基づいて更新し、更新された確率分布に基づいて、前記相対関係を推定する。なお、ベイズフィルタを用いる場合に、前記相対関係が一意に求められないときにも、確からしい前記相対関係を、確率分布を利用して求めることができる。この確率分布を絞り込むために、他の計測手段により、第１および第２の移動体３、５の位置や姿勢を計測し、これらの計測値をさらに用いてもよい。

以下、ベイズフィルタとして、カルマンフィルタとパーティクルフィルタを用いた例を順に説明する。

（カルマンフィルタ）
ベイズフィルタとしてカルマンフィルタを用いる場合には、次のようにする。

まず、前記移動体の位置および姿勢を内部状態とする。例えば、第１の移動体３に設定された前記基点Ｐ１と前記方位点Ｐ２に対して静止している座標系における第２の移動体５の位置と姿勢を内部状態とする。すなわち、ここでの例では、内部状態は、第１の移動体３に対する第２の移動体５の相対位置と相対姿勢である。
演算部１３は、前記確率分布を用いたカルマンフィルタにより、第１の方位センサ９が計測した前記方位θ_１に基づいて内部状態と該内部状態の分散共分散行列を更新するとともに、第２の方位センサ１１が計測した前記方位θ_２に基づいて内部状態とその分散共分散行列を更新し、更新された内部状態（相対姿勢）から前記相対関係を推定し、または、以下で詳しく説明する具体例のように、更新された内部状態を前記相対関係とする。なお、カルマンフィルタを用いる場合には、前記確率分布は、内部状態と分散共分散行列で表わされる。

なお、分散共分散行列が示す内部状態の確からしさが、しきい値以上になるまで、方位θ_１と方位θ_２の計測を繰り返すのがよい。この場合、演算部１３は、方位θ_１が計測される度に、カルマンフィルタにより、当該方位θ_１に基づいて内部状態とその分散共分散行列を更新するとともに、方位θ_２が計測される度に、カルマンフィルタにより、当該方位θ_２に基づいて内部状態とその分散共分散行列を更新する。分散共分散行列が示す内部状態の確からしさが、しきい値以上になったら、演算部１３は、最新の内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。

以下において、カルマンフィルタを用いた場合の具体例を説明する。

まず、次の［数１］で示すように、内部状態Ｘ_ｔを定義するとともに、内部状態Ｘ_ｔの初期値と、内部状態Ｘ_ｔの初期分散共分散行列ＣｏｖＸ_ｔを定義しておく。

［数１］において、ｘ、ｙ、ｚは、現在において、第１の移動体３から見た第２の移動体５の相対位置を示す。すなわち、ｘ、ｙ、ｚは、それぞれ、第１の移動体３に固定された３次元座標系Ａにおける互いに直交するｘ軸とｙ軸とｚ軸の座標値である。この３次元座標系Ａのｘ軸は、第１基準方向Ｄ１に相当する。すなわち、この３次元座標系Ａの原点が基点Ｐ１であり、ｘ軸上に方位点Ｐ２が存在する。
［数１］において、ｒ_ｘ、ｒ_ｙ、ｒ_ｚは、現在において、第１の移動体３から見た第２の移動体５の姿勢を示す角度（ラジアン）である。すなわち、３次元座標系Ａにおいて、第２の移動体５を、ｘ軸と平行であり第２の移動体５（例えば、基点Ｐ３）を通る軸回りにその基準姿勢からｒ_ｘだけ回転させ、次に、ｙ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒ_ｙだけ回転させ、最後に、ｚ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒ_ｚだけ回転させると、第１の移動体３から見た現在における第２の移動体５の姿勢となる。基準姿勢とは、例えば、第２基準方向Ｄ２の向きが第１基準方向Ｄ１の向きと同じ（平行）になっている第２の移動体５の姿勢である。なお、本発明によると、回転させる順序を変えたり、別のオイラー角表現や回転行列などにより、第１の移動体３から見た第２の移動体５の姿勢を表してもよい。この場合、後述の式（２０）も変更する。

この例では、θ_１として、第１の方位センサ９は、θ_１ｚとθ_１ｘｙとを計測する。
θ_１ｚは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、ｚ軸回りの周方向に関して、第１の移動体３から見て、第１基準方向Ｄ１（ｘ軸）に対して、第２の移動体５が存在する方位（ラジアン）である。すなわち、θ_１ｚは、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３をｘ−ｙ平面に正射影した点Ｐ３_ｘｙと３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分の、第１基準方向Ｄ１（ｘ軸）に対する方位である。
θ_１ｘｙは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３と前記３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。

θ_１ｚが計測されたら、演算部１３は、次の式（２）（３）により、Ｘ_ｔとＣｏｖＸ_ｔを更新する。なお、 ＣｏｖＸ_ｔは、Ｘ_ｔの分散共分散行列を示す（以下同様）。

Ｘ_ｔ＝Ｘ_ｔ−Ｋ_１（Ｙ_１−θ_１ｚ） ・・・（２）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_１Ｂ_１）・ＣｏｖＸ_ｔ ・・・（３）

ここで、式（２）において、左辺のＸ_ｔは、当該式により更新されたＸ_ｔであり、右辺のＸ_ｔは、当該式によりＸ_ｔを更新する直前のＸ_ｔである。また、式（３）において、左辺のＣｏｖＸ_ｔは、当該式により更新されたＣｏｖＸ_ｔであり、右辺のＣｏｖＸ_ｔは、当該式によりＣｏｖＸ_ｔを更新する直前のＣｏｖＸ_ｔである。また、式（３）において、Ｉは、単位行列を示す。

Ｋ_１は、カルマンゲインであり、次の式（４）で表わされる。

Ｋ_１＝ＣｏｖＸ_ｔ・Ｂ_１ ^Ｔ・Ｖ_１ ^−１ ・・・（４）

Ｂ_１は、次の［数２］の式（５）で表わされる偏微分行列であり、その上付き文字「Ｔ」は転置行列を示す。
上式（４）のＶ_１は、次の式で表わされ、その上付き数字「-１」は逆行列を示す。


Ｖ_１＝Ｂ_１・ＣｏｖＸ_ｔ・Ｂ_１ ^Ｔ＋Ｒ ・・・（６）

ここで、Ｒは、方位の計測誤差として想定される分散値である（以下同様）。

上式（２）のＹ_１は、次の式で表わされる。

Ｙ_１＝ｇ_１（Ｘ_ｔ）＝ｔａｎ^−１（ｙ／ｘ） ・・・（７）

θ_１ｘｙが計測されたら、演算部１３は、次の式（８）（９）により、Ｘ_ｔとＣｏｖＸ_ｔを更新する。

Ｘ_ｔ＝Ｘ_ｔ−Ｋ_２（Ｙ_２−θ_１ｘｙ） ・・・（８）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_２Ｂ_２）・ＣｏｖＸ_ｔ ・・・（９）

ここで、式（８）において、左辺のＸ_ｔは、当該式により更新されたＸ_ｔであり、右辺のＸ_ｔは、当該式によりＸ_ｔを更新する直前のＸ_ｔである。また、式（９）において、左辺のＣｏｖＸ_ｔは、当該式により更新されたＸ_ｔであり、右辺のＣｏｖＸ_ｔは、当該式によりＣｏｖＸ_ｔを更新する直前のＣｏｖＸ_ｔである。また、式（９）において、Ｉは、単位行列を示す。

Ｋ_２は、カルマンゲインであり、次の式（１０）で表わされる。

Ｋ_２＝ＣｏｖＸ_ｔ・Ｂ_２ ^Ｔ・Ｖ_２ ^−１ ・・・（１０）

Ｂ_２は、次の［数３］の式（１１）で表わされる偏微分行列である。
上式（１０）のＶ_２は、次の式（１２）で表わされる。

Ｖ_２＝Ｂ_２・ＣｏｖＸ_ｔ・Ｂ_２ ^Ｔ＋Ｒ ・・・（１２）

上式（８）のＹ_２は、次の［数４］の式（１３）で表わされる。

この例では、θ_２として、第２の方位センサ１１は、θ_２ｚとθ_２ｘｙとを計測する。
θ_２ｚは、図５（Ｂ）に示すように、互いに直交するｘ軸、ｙ軸、ｚ軸を有し第２の移動体５に固定された３次元座標系Ｂにおいて、ｚ軸回りの周方向に関して、第２の移動体５から見て、第２基準方向Ｄ２（ｘ軸）に対して、第１の移動体３が存在する方位（ラジアン）である。すなわち、θ_２ｚは、３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１をｘ−ｙ平面に正射影した点Ｐ１_ｘｙと３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分の、第２基準方向Ｄ２（ｘ軸）に対する方位である。
θ_２ｘｙは、図５（Ｂ）に示すように、前記３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１と前記３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。

θ_２ｚが計測されたら、演算部１３は、次の式（１４）（１５）により、Ｘ_ｔとＣｏｖＸ_ｔを更新する。

Ｘ_ｔ＝Ｘ_ｔ−Ｋ_３（Ｙ_３−θ_２ｚ） ・・・（１４）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_３Ｂ_３）・ＣｏｖＸ_ｔ ・・・（１５）

ここで、式（１４）において、左辺のＸ_ｔは、当該式により更新されたＸ_ｔであり、右辺のＸ_ｔは、当該式によりＸ_ｔを更新する直前のＸ_ｔである。また、式（１５）において、左辺のＣｏｖＸ_ｔは、当該式により更新されたＣｏｖＸ_ｔであり、右辺のＣｏｖＸ_ｔは、当該式によりＣｏｖＸ_ｔを更新する直前のＣｏｖＸ_ｔである。また、式（１５）において、Ｉは、単位行列を示す。

Ｋ_３は、カルマンゲインであり、次の式（１６）で表わされる。

Ｋ_３＝ＣｏｖＸ_ｔ・Ｂ_３ ^Ｔ・Ｖ_３ ^−１ ・・・（１６）

Ｂ_３は、次の［数５］の式（１７）で表わされる偏微分行列である。
上式（１６）のＶ_３は、次の式（１８）で表わされる。

Ｖ_３＝Ｂ_３・ＣｏｖＸ_ｔ・Ｂ_３ ^Ｔ＋Ｒ ・・・（１８）

上式（１４）のＹ_３は、次の式（１９）で表わされる。

Ｙ_３＝ｇ_３（Ｘ_ｔ）＝ｔａｎ^−１（ｙ_Ｂ／ｘ_Ｂ） ・・・（１９）

ここで、ｘ_Ｂとｙ_Ｂは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値である。そのため、ｘ_Ｂとｙ_Ｂは、次の［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。
なお、この式（２０）において、ｃとｓは、それぞれ、余弦関数（ｃｏｓ）と正弦関数（ｓｉｎ）を表わす。また、この式（２０）において、ｚ_Ｂは、３次元座標系Ｂにおけるｚ軸の座標値である。

θ_２ｘｙが計測されたら、演算部１３は、次の式（２１）（２２）により、Ｘ_ｔとＣｏｖＸ_ｔを更新する。

Ｘ_ｔ＝Ｘ_ｔ−Ｋ_４（Ｙ_４−θ_２ｘｙ） ・・・（２１）

ＣｏｖＸ_ｔ＝（Ｉ−Ｋ_４Ｂ_４）・ＣｏｖＸ_ｔ ・・・（２２）

ここで、式（２１）において、左辺のＸ_ｔは、当該式により更新されたＸ_ｔであり、右辺のＸ_ｔは、当該式によりＸ_ｔを更新する直前のＸ_ｔである。また、式（２２）において、左辺のＣｏｖＸ_ｔは、当該式により更新されたＣｏｖＸ_ｔであり、右辺のＣｏｖＸ_ｔは、当該式によりＣｏｖＸ_ｔを更新する直前のＣｏｖＸ_ｔである。また、式（２２）において、Ｉは、単位行列を示す。

Ｋ_４は、カルマンゲインであり、次の式（２３）で表わされる。

Ｋ_４＝ＣｏｖＸ_ｔ・Ｂ_４ ^Ｔ・Ｖ_４ ^−１ ・・・（２３）

Ｂ_４は、次の［数７］の式（２４）で表わされる偏微分行列である。
上式（２３）のＶ_４は、次の式（２５）で表わされる。

Ｖ_４＝Ｂ_４・ＣｏｖＸ_ｔ・Ｂ_４ ^Ｔ＋Ｒ ・・・（２５）

上式（２１）のＹ_４は、次の［数８］の式（２６）で表わされる。
この式におけるｘ_Ｂとｙ_Ｂとｚ_Ｂは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値とｚ軸の座標であり、上述した［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。

上述のように更新された内部状態を、前記相対関係（前記相対姿勢）とする。すなわち、更新された内部状態Ｘ_ｔのｒ_ｘ、ｒ_ｙ、ｒ_ｚを前記相対関係とする。

上述の具体例では、３次元座標系Ａと３次元座標系Ｂをそれぞれ第１および第２の移動体３、５に固定していたが、任意の座標系を用いてよい。例えば、静止座標系を用いてもよい。この静止座標系に対して、第１および第２の移動体３、５は移動し姿勢を変更する。この場合、内部状態は、１２の変数からなる。１２の変数は、第１の移動体３の位置を示す３つの変数と、第１の移動体３の姿勢を示す３つの変数と、第２の移動体５の位置を示す３つの変数と、第２の移動体５の姿勢を示す３つの変数とからなる。そこで、好ましくは、上述の第１および第２の方位センサ９、１１が計測した方位に加えて、第１および第２の移動体３、５間の距離の計測値や、静止座標系における移動体３、５の位置の計測値を用いて、カルマンフィルタにより内部状態を更新する。更新された内部状態から前記相対関係を推定する。ただし、移動体３、５間の距離の計測値と、移動体３、５の位置の計測値の一方または両方が得られなくても、カルマンフィルタにより内部状態を更新し、更新された内部状態から前記相対関係を推定できる。

（パーティクルフィルタ）
ベイズフィルタとしてパーティクルフィルタを用いる場合には、次のようにする。

（ａ）まず、第１の移動体３の位置および姿勢と、第２の移動体５の位置および姿勢とを１つのサンプルとして、演算部１３は、第１および第２の移動体３、５が取り得るサンプルを多数生成する。これらのサンプルは、第１および第２の移動体３、５が移動する空間の座標系で表わされてよい。すなわち、この座標系に対して、第１および第２の移動体３、５が移動する。これらのサンプルは、前記方位θ_１と方位θ_２の一方を計測した時刻のものである。
（ｂ）演算部１３は、多数のサンプルから前記方位θ_１と方位θ_２の一方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ_１と方位θ_２の一方に基づいて、各サンプルが実際に存在する確率を前記確率分布として取得（更新）し、当該確率に基づいて、多数のサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ_１と方位θ_２の一方に適合する度合いが高いほど、各サンプルが実際に存在する確率が高いものであってもよいし、前記方位θ_１と方位θ_２の一方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｃ）次に、演算部１３は、前記（ｂ）の処理で抽出した各サンプルについて、前記方位θ_１と方位θ_２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。すなわち、前記（ｂ）の処理で抽出した各サンプルについて、当該サンプルである第１および第２の移動体３、５の位置および姿勢が、方位θ_１と方位θ_２の他方を計測した時にとっていた値を、各移動体３、５の運動モデルに基づいて求め、求めた当該値により、当該他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を予測する。
（ｄ）さらに、演算部１３は、前記（ｂ）の処理で抽出したサンプルから前記方位θ_１と方位θ_２の他方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ_１と方位θ_２の他方と、予測した第１および第２の移動体３、５の相対位置と相対姿勢とに基づいて、前記抽出した各サンプルが実際に存在する確率を更新し、当該更新した確率に基づいて、前記抽出したサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ_１と方位θ_２の他方との適合度が高いサンプルほど、高くなるものであってもよいし、前記方位θ_１と方位θ_２の他方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｅ）その後、演算部１３は、前記（ｄ）で抽出したサンプルに基づいて、前記相対関係を推定する。当該サンプルの全ては、第１の移動体３に対する第２の移動体５の相対姿勢が等しいものとなっていると考えられる。

方位θ_１と方位θ_２の一方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｂ）の処理において、当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ_１と方位θ_２の一方に適合するサンプルを抽出してもよい。
同様に、方位θ_１と方位θ_２の他方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｄ）の処理において、予測した第１および第２の移動体３、５の前記相対位置に基づいて当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ_１と方位θ_２の他方に適合するサンプルを抽出してもよい。
なお、第１および第２の移動体３、５間の距離は、例えば、第１または第２の移動体３、５に設置されたレーザ距離計により計測してよい。このレーザ距離計は、この測定時刻またはその直前に得られた方位θ_１と方位θ_２に相当する方位において、第１および第２の移動体３、５間の距離を計測してよい。

また、前記（ｂ）の処理において、演算部１３は、前記（ｂ）で抽出したサンプルに近いサンプルを新たに生成してもよい。すなわち、演算部１３は、抽出したサンプルと、移動体３、５の位置と姿勢が近いサンプルを新たに生成してもよい。この場合、前記（ｃ）の処理では、演算部１３は、前記新たに生成した各サンプルについても、前記方位θ_１と方位θ_２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。さらに、前記（ｄ）の処理では、演算部１３は、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルとについて予測した相対位置と相対姿勢に基づいて、上述と同様に、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルから、前記方位θ_１と方位θ_２の他方に適合するサンプルを抽出する。

方位θ_１と方位θ_２を繰り返し計測し、方位θ_１と方位θ_２を計測する度に、前記（ａ）を省略して、当該方位θ_１と方位θ_２について前記（ｂ）（ｃ）（ｄ）の処理を行ってもよい。この場合、方位θ_１と方位θ_２が再び計測されたら、直前の前記（ｄ）で抽出されたサンプルについて、当該方位θ_１と方位θ_２の一方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。次いで、再度の前記（ｂ）において、これらの予測した相対位置と相対姿勢に基づいて、上述と同様に、直前の前記（ｄ）で抽出されたサンプルから前記方位θ_１と方位θ_２の一方に適合するサンプルを抽出する。その後、上述と同様に、前記（ｃ）（ｄ）の処理を行う。なお、前記（ｅ）の処理では、最後に行った前記（ｄ）の処理で抽出されたサンプルに基づいて、前記相対関係を求める。

［移動体の制御］
次に、各移動体の移動制御について説明する。

移動体３、５毎に制御装置が設けられ、該制御装置は、対応する移動体３または５の位置に基づいて、該移動装置が目標位置へ移動するように、該移動体の移動速度と移動方向を制御する。
前記制御装置は、対応する移動体３または５に設置されてよい。移動体３、５は、例えば、走行車輪や歩行脚により路面上を移動する移動装置、空中を移動する飛行体、または、宇宙空間を移動する宇宙機である。

このような制御を行うために、移動体３、５が移動する空間に固定された静止座標系における移動体３、５の、位置を検出する位置検出装置７が設けられる。この位置検出装置７は、適宜の手段で構成されてよいが、図１の例のように、移動体３、５が移動する空間に設置された３次元レーザレーダまたはカメラであってよい。
各移動体３、５の前記制御装置は、位置検出装置７により検出した移動体３、５の位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、対応する移動体３または５の移動速度と移動方向を制御する。

このような移動体の制御方法として、例えば、次の制御方法１、２を採用できる。

（制御方法１）
第２の移動体５に第１の移動体３を追従させる場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第２の移動体５に第１の移動体３が追従して移動するように第１の移動体３の移動速度と移動方向を制御する。一方、第２の移動体５の前記制御装置は、位置検出装置７により検出した第２の移動体５の、位置に基づいて、第２の移動体５が目標位置へ移動するように、第２の移動体５の移動速度と移動方向を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ_２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。

（制御方法２）
第１の移動体３と第２の移動体５の衝突を避ける場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第１および第２の移動体３、５が互いに衝突しないように第１の移動体３の移動を制御する。例えば、第１の移動体３の前記制御装置は、前記相対位置と相対姿勢に基づいて、第１および第２の移動装置が互いに衝突する可能性があるかを判断し、この可能性があると判断した場合には、この衝突を避けるように第１の移動体３の移動速度と移動方向の一方または両方を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ_２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。

本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。

上述の実施形態では、各方位センサ９、１１は、カメラと画像処理装置により構成されたが、他の手段で構成されてもよい。例えば、各方位センサ９、１１は、レーザレーダにより構成されてもよい。

３ 第１の移動体、５ 第２の移動体、７ 位置検出装置、９ 第１の方位センサ、１１ 第２の方位センサ、１３ 演算部、１０ 相対姿勢計測装置

Claims (5)

1. 第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
   （Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
   （Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ_１を計測し、
   （Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ_２を計測し、
   （Ｄ）計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法。
2. 前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ_１と方位θ_２に基づいて更新し、
   更新された確率分布に基づいて、前記相対関係を推定する、ことを特徴とする請求項１に記載の移動体の相対姿勢計測方法。
3. 前記移動体の位置および姿勢を内部状態とし、
   該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
   前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ_１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ_２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
4. 前記（Ｄ）において、
   第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
   前記方位θ_１と方位θ_２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
   前記抽出した各サンプルについて、前記方位θ_１と方位θ_２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
   前記方位θ_１と方位θ_２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
   当該抽出したサンプルに基づいて、前記相対関係を推定する、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
5. 第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
   第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
   第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ_１を計測し、
   第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ_２を計測し、
   計測した方位θ_１と方位θ_２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置。