移動体の相対姿勢計測方法と装置
【課題】２つの移動体の相対姿勢を精度よく求めることができる手段を提供する。
【解決手段】第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。計測した方位θ１と方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係θｔを、第１および第２の移動体の相対姿勢として求める。
【解決手段】第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。計測した方位θ１と方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係θｔを、第１および第２の移動体の相対姿勢として求める。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法と装置に関する。
【背景技術】
【０００２】
複数の移動体が、互いに独立してエリア内を移動する場合に、複数の移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御などを行うことがある。
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。
【０００３】
上述の制御を行うために、移動体の位置と姿勢を、次の方法Ａ、ＢまたはＣにより計測している。
【０００４】
方法Ａでは、移動体に、速度センサやジャイロセンサなどを設置する。速度センサによる速度計測値を時間積分して、移動体の位置を求め、ジャイロセンサによる角速度計測値を時間積分して、移動体の姿勢を求める。
【０００５】
方法Ｂでは、移動体に、外界センサを搭載する。この外界センサにより、移動体の外部において既知の位置に存在する指標の方向と、該指標までの距離を計測する。この計測結果を、地図データと照合する。この照合に基づいて、移動体の自己位置と姿勢を求める。例えば、外界センサとして３次元レーザレーダやカメラを用いる。なお、下記の特許文献１では、宇宙機である移動体の姿勢として、当該移動体の進行方向と前記指標に相当する地球や太陽との相対関係を検出している。
【０００６】
方法Ｃでは、移動体の外部に設置された計測機器（例えば、カメラ）により移動体を含む画像を取得し、この画像に基づいて移動体の位置と姿勢を求める。
【０００７】
なお、後述する本発明の実施形態で使用可能な技術が、下記の特許文献２〜４に記載されている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第２５１４９８７号
【特許文献２】特開２００９−３３３６６号公報
【特許文献３】特開２００７−３２３３号公報
【特許文献４】特開２０１１−２８４１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、移動体の位置と姿勢のうち、姿勢を精度よく計測することは難しい。
【００１０】
上述した方法Ａでは、ジャイロセンサの計測誤差が積分されることにより、移動体の姿勢の計測精度が低下する可能性がある。
【００１１】
上述した方法Ｂでは、３次元レーザレーダやカメラにより指標を認識する必要がある。そのため、指標を設定できない場合には、移動体の姿勢を計測できない。
【００１２】
上述した方法Ｃでは、移動体の姿勢を得るためには、計測機器により、移動体における３つの位置を互いに区別して計測する必要がある。従って、移動体と計測機器との距離が大きくなると、移動体における３つの位置を互いに区別して計測することが困難となる。
【００１３】
一方、２つの移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御を行う場合には、２つの移動体の相対姿勢が得られればよい。すなわち、移動体が移動するエリアに固定された静止座標系から見た各移動体の姿勢を求めることは必ずしも必要ではない。
【００１４】
そこで、本発明の目的は、２つの移動体の相対姿勢を精度よく求めることができる方法と装置を提供することにある。
【課題を解決するための手段】
【００１５】
上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
（Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
（Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
（Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
（Ｄ）計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法が提供される。
【００１６】
本発明の好ましい実施形態によると、前記（Ｂ）を行う時の前記相対姿勢は、前記（Ｃ）を行う時の前記相対姿勢と同じであり、
前記（Ｂ）（Ｃ）で計測された方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との前記相対関係を幾何学的に算出する。
【００１７】
本発明の別の実施形態によると、前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、
更新された確率分布に基づいて、前記相対関係を推定する。
【００１８】
好ましくは、前記移動体の位置および姿勢を内部状態とし、
該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。
【００１９】
好ましい別の例では、前記（Ｄ）において、
第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
前記抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
前記方位θ１と方位θ２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
当該抽出したサンプルに基づいて、前記相対関係を推定する。
【００２０】
また、上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置が提供される。
【発明の効果】
【００２１】
上述した本発明によると、第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
この方法では、第１の方位センサは、第２の移動体の方位を認識できればよく、第２の移動体における３つの位置を互いに区別して計測する必要がない。第２の方位センサも同様である。
よって、第１の移動体と第２の移動体との距離が大きくても、相対姿勢を高精度に求めることができる。
【図面の簡単な説明】
【００２２】
【図１】本発明の実施形態による相対姿勢計測装置を示す。
【図２】（Ａ）は、第１の移動体に固定した２次元座標系を示し、（Ｂ）は、第２の移動体に固定した２次元座標系を示す。
【図３】本発明の実施形態による相対姿勢計測方法を示すフローチャートである。
【図４】第１の移動体に固定された３次元座標系Ａと、第２の移動体に固定された３次元座標系Ｂを示す。
【図５】（Ａ）は、第１の方位センサが計測する方位θ１ｚ、θ１ｘｙを示し、（Ｂ）は、第２の方位センサが計測する方位θ２ｚ、θ２ｘｙを示す。
【発明を実施するための形態】
【００２３】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２４】
図１は、本発明の実施形態による相対姿勢計測装置１０の構成図である。相対姿勢計測装置１０は、第１の移動体３に対する第２の移動体５の相対姿勢を計測する。
図１の例では、第１および第２の移動体３、５は、路面を走行するものであり、図１は、各移動体３、５を上方から見た平面図である。なお、図１において、各移動体３、５の位置を検出する位置検出装置７（３次元レーザレーダ）が、移動体３、５以外の固定位置に設けられている。
【００２５】
相対姿勢計測装置１０は、第１の方位センサ９と第２の方位センサ１１と演算部１３とを備える。
【００２６】
第１の方位センサ９は、第１の移動体３に設けられる。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。ここで、第１基準方向Ｄ１とは、第１の移動体３に設定された基点Ｐ１から、該基点Ｐ１と間隔をおいて第１の移動体３に設定された方位点Ｐ２へ向かう方向である。好ましくは、第１基準方向Ｄ１は、第１の移動体３の前後方向であって、第１の移動体３の後方から前方へ向かう方向である。ただし、第１基準方向Ｄ１は、第１の移動体３の前後方向から既知の角度だけ傾いた方向であってもよい。
【００２７】
第１の方位センサ９は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第１基準方向Ｄ１を向くように、または、第１基準方向Ｄ１に対して既知の角度だけ傾いた方向を向くように第１の移動体３に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第２の移動体５との位置関係に基づいて、第２の移動体５が位置する方位θ１を求める。この時、画像処理装置は、第２の移動体５に設けられた指標を、前記画像内において第２の移動体５の位置として特定してよい。なお、好ましくは、第１の移動体３から見てすべての方向の画像が得られるように、全方向の視野を有する１つのカメラを第１の移動体３に設置するか、もしくは、異なる方向を向く複数のカメラを第１の移動体３に設置するのがよい。
【００２８】
ここで、θ１は、例えば、第１の移動体３に固定された２次元座標系Ａで表現されてよい。例えば、２次元座標系Ａは、図２（Ａ）に示すように、その原点が基点Ｐ１であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第１基準方向Ｄ１に相当する。すなわち、ｘ軸上に方位点Ｐ２が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ１は、図２（Ａ）において、第１基準方向Ｄ１（ｘ軸）に対する角度であり、基点Ｐ１を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ１を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。
【００２９】
第２の方位センサ１１は、第２の移動体５に設けられる。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。ここで、第２基準方向Ｄ２とは、第２の移動体５に設定された基点Ｐ３から、該基点Ｐ３と間隔をおいて第２の移動体５に設定された方位点Ｐ４へ向かう方向である。好ましくは、第２基準方向Ｄ２は、第２の移動体５の前後方向であって、第２の移動体５の後方から前方へ向かう方向である。ただし、第２基準方向Ｄ２は、第２の移動体５の前後方向から既知の角度だけ傾いた方向であってもよい。
【００３０】
第２の方位センサ１１は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第２基準方向Ｄ２を向くように、または、第２基準方向Ｄ２に対して既知の角度だけ傾いた方向を向くように第２の移動体５に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第１の移動体３との位置関係に基づいて、第１の移動体３が位置する方位θ２を求める。この時、画像処理装置は、第１の移動体３に設けられた指標を、前記画像内において第１の移動体３の位置として特定してよい。なお、好ましくは、第２の移動体５から見てすべての方位の画像が得られるように、全方向の視野を有する１つのカメラを第２の移動体５に設置するか、もしくは、異なる方向を向く複数のカメラを第２の移動体５に設置するのがよい。
【００３１】
ここで、θ２は、例えば、第２の移動体５に固定された２次元座標系Ｂで表現されてよい。例えば、２次元座標系Ｂは、図２（Ｂ）に示すように、その原点が基点Ｐ３であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第２基準方向Ｄ２に相当する。すなわち、ｘ軸上に方位点Ｐ４が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ２は、図２（Ｂ）において、第２基準方向Ｄ２（ｘ軸）に対する角度であり、基点Ｐ３を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ３を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。
【００３２】
各移動体３、５に設ける上述の指標として、例えば、次のものを用いてよい。
一例では、指標は、特許文献２のように、特定のパターンで輝度が変化する発光ダイオードである。この場合、方位センサ９、１１は、カメラを用いて、短い設定時間にわたる動画を画像として撮像し、画像処理装置を用いて、画像において、特定のパターンで輝度が変化する画素の位置を、指標の位置として特定する。
別の例では、指標は、特許文献３、４に記載されたような幾何学的模様である。この場合、方位センサ９、１１の画像処理装置は、幾何学的模様と同じ模様を予め記憶しており、カメラを用いて得た画像において、予め記憶した当該模様と同じ模様の部分を、指標の位置として特定する。
【００３３】
上述の指標は、第１および第２の移動体３、５の外面（例えば、外周面）に、間隔をおいて取り付けられていてよい。
【００３４】
演算部１３は、第１の方位センサ９が計測した方位θ１と、第１の方位センサ９が計測した方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係を求める。求めた相対関係が、第１の移動体３に対する第２の移動体５の相対姿勢となる。相対関係は、例えば、図１のように、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度θｔで表わされる。このなす角度θｔは、第１基準方向Ｄ１、または、この方向Ｄ１に平行な方向と、第２基準方向Ｄ２、または、この方向Ｄ２に平行な方向とのなす角度を意味する。
【００３５】
図３は、上述した相対姿勢計測装置１０を用いた相対姿勢計測方法を示すフローチャートである。
【００３６】
ステップＳ１において、第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。
ステップＳ２において、第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。
ステップＳ３において、第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。
ステップＳ４において、演算部１３は、計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
なお、ステップＳ２、Ｓ３は、移動体３、５が移動している時に行われてよい。
【００３７】
以下、ステップＳ４で行われる演算部１３による演算方法１、２を詳しく説明する。
【００３８】
［演算方法１］
演算方法１は、ステップＳ２を行う時の前記相対姿勢が、ステップＳ３を行う時の前記相対姿勢と同じである場合に採用される。このような場合としては、ステップＳ２を行う時とステップＳ３を行う時との間で、第１および第２の移動体３、５の姿勢と位置が変化していない場合がある。演算方法１を行う他の場合として、ステップＳ２とステップＳ３を同じ時刻に行う場合がある。
【００３９】
演算部１３は、ステップＳ２とステップＳ３でそれぞれ計測した方位θ１と方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係（なす角度θｔ）を幾何学的演算により算出する。ここでは、上述の図２のように、方位θ１、θ２は、上述の２次元座標系Ａ、Ｂで表現できるものとする。なお、図１において、Ｄ１ｐは、第１基準方向と平行であって基点Ｐ３を通る直線である。
演算部１３は、方位θ１と方位θ２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度をθｔを、次の式（１）により求める。なお、この式（１）において、θ１とθ２とθｔの単位はラジアンである。
θｔ＝θ１−θ２−π ・・・（１）
【００４０】
上述では、第１および第２の移動体３、５が、２次元平面である路面を移動する場合について説明したので、ここでは、方位θ１、θ２は、２次元座標系Ａ、Ｂで表わした。しかし、第１および第２の移動体３、５が、３次元空間内を移動する場合には、演算部１３は、３次元座標系で表わされた方位θ１、θ２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度を次のように求める。
【００４１】
図４は、移動体３、５が移動する空間において、これらの移動体３、５にそれぞれ固定された３次元座標系Ａ、Ｂを示す。各３次元座標系Ａ，Ｂは、互いに直交するｘ軸とｙ軸とｚ軸を有する。上述の第１基準方向Ｄ１は、３次元座標系Ａのｘ軸に一致し、上述の第２基準方向Ｄ２は、３次元座標系Ｂのｘ軸に一致する。
【００４２】
図４において、第１の方位センサ９は、上述の方位θ１として方位θ１ｚとθ１ｘｙを計測し、第２の方位センサ１１は、上述の方位θ２として方位θ２ｚとθ２ｘｙを計測する。なお、図４において、Ｐ１ｘｙは、３次元座標系Ａの原点Ｐ１を３次元座標系Ｂのｘ−ｙ平面に正射影した点であり、Ｐ３ｘｙは、３次元座標系Ｂの原点Ｐ３を３次元座標系Ａのｘ−ｙ平面に正射影した点である。
【００４３】
また、第１基準方向Ｄ１をベクトルｖ１で表わし、第１基準方向Ｄ２をベクトルｖ２で表わし、Ｒｔを回転行列として、
ｖ１＝Ｒｔｖ２
が成り立つとする。この場合、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係は、上記のＲｔで表わすことができる。ここで、Ｒｔは、例えば、
Ｒｔ＝Ｒ１ｚ（π）Ｒ１ｚ（-θ１ｚ）Ｒ１ｘｙ（θ１ｘｙ）Ｒ２ｘｙ（θ２ｘｙ）Ｒ２ｚ（θ２ｚ）
で表わされる。
Ｒ２ｚ（θ２ｚ）は、空間上の各点を、３次元座標系Ｂのｚ軸回りに、θ２ｚだけ回転させる行列である。Ｒ２ｘｙ（θ２ｘｙ）は、３次元座標系ＢをＲ２ｚ（θ２ｚ）により回転させた座標系Ｂ１のｙ軸回りに、空間上の各点を、θ２ｘｙだけ回転させる行列である。Ｒ１ｘｙ（θ１ｘｙ）は、３次元座標系Ａのｚ軸回りに、座標系Ａをθ１ｚだけ回転させた座標系Ａ１のｙ軸回りに、空間上の各点を、θ１ｘｙだけ回転させる行列である。Ｒ１ｚ（−θ１ｚ）は、３次元座標系Ａのｚ軸回りに、空間上の各点を、−θ１ｚだけ回転させる行列である。Ｒ１ｚ（π）は、空間上の各点を、３次元座標系Ａのｚ軸回りに、π（ラジアン）だけ回転させる行列である。なお、各回転角θは、大きさを表しており、θは、各軸の正の方向から見て反時計回りの回転を示し、−θは、各軸の正の方向から見て時計回りの回転を示している。
【００４４】
［演算方法２］
演算方法２は、第１の方位センサ９と第２の方位センサ１１が、異なる時刻に、または、同じ時刻に、上述した方位θ１と方位θ２を計測する場合に採用される。
この場合、演算部１３は、ベイズフィルタ（ベイズの定理）を用いて、前記相対関係を推定する。すなわち、演算部１３は、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、更新された確率分布に基づいて、前記相対関係を推定する。なお、ベイズフィルタを用いる場合に、前記相対関係が一意に求められないときにも、確からしい前記相対関係を、確率分布を利用して求めることができる。この確率分布を絞り込むために、他の計測手段により、第１および第２の移動体３、５の位置や姿勢を計測し、これらの計測値をさらに用いてもよい。
【００４５】
以下、ベイズフィルタとして、カルマンフィルタとパーティクルフィルタを用いた例を順に説明する。
【００４６】
（カルマンフィルタ）
ベイズフィルタとしてカルマンフィルタを用いる場合には、次のようにする。
【００４７】
まず、前記移動体の位置および姿勢を内部状態とする。例えば、第１の移動体３に設定された前記基点Ｐ１と前記方位点Ｐ２に対して静止している座標系における第２の移動体５の位置と姿勢を内部状態とする。すなわち、ここでの例では、内部状態は、第１の移動体３に対する第２の移動体５の相対位置と相対姿勢である。
演算部１３は、前記確率分布を用いたカルマンフィルタにより、第１の方位センサ９が計測した前記方位θ１に基づいて内部状態と該内部状態の分散共分散行列を更新するとともに、第２の方位センサ１１が計測した前記方位θ２に基づいて内部状態とその分散共分散行列を更新し、更新された内部状態（相対姿勢）から前記相対関係を推定し、または、以下で詳しく説明する具体例のように、更新された内部状態を前記相対関係とする。なお、カルマンフィルタを用いる場合には、前記確率分布は、内部状態と分散共分散行列で表わされる。
【００４８】
なお、分散共分散行列が示す内部状態の確からしさが、しきい値以上になるまで、方位θ１と方位θ２の計測を繰り返すのがよい。この場合、演算部１３は、方位θ１が計測される度に、カルマンフィルタにより、当該方位θ１に基づいて内部状態とその分散共分散行列を更新するとともに、方位θ２が計測される度に、カルマンフィルタにより、当該方位θ２に基づいて内部状態とその分散共分散行列を更新する。分散共分散行列が示す内部状態の確からしさが、しきい値以上になったら、演算部１３は、最新の内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。
【００４９】
以下において、カルマンフィルタを用いた場合の具体例を説明する。
【００５０】
まず、次の［数１］で示すように、内部状態Ｘｔを定義するとともに、内部状態Ｘｔの初期値と、内部状態Ｘｔの初期分散共分散行列ＣｏｖＸｔを定義しておく。
【００５１】
【数１】
【００５２】
［数１］において、ｘ、ｙ、ｚは、現在において、第１の移動体３から見た第２の移動体５の相対位置を示す。すなわち、ｘ、ｙ、ｚは、それぞれ、第１の移動体３に固定された３次元座標系Ａにおける互いに直交するｘ軸とｙ軸とｚ軸の座標値である。この３次元座標系Ａのｘ軸は、第１基準方向Ｄ１に相当する。すなわち、この３次元座標系Ａの原点が基点Ｐ１であり、ｘ軸上に方位点Ｐ２が存在する。
［数１］において、ｒｘ、ｒｙ、ｒｚは、現在において、第１の移動体３から見た第２の移動体５の姿勢を示す角度（ラジアン）である。すなわち、３次元座標系Ａにおいて、第２の移動体５を、ｘ軸と平行であり第２の移動体５（例えば、基点Ｐ３）を通る軸回りにその基準姿勢からｒｘだけ回転させ、次に、ｙ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒｙだけ回転させ、最後に、ｚ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒｚだけ回転させると、第１の移動体３から見た現在における第２の移動体５の姿勢となる。基準姿勢とは、例えば、第２基準方向Ｄ２の向きが第１基準方向Ｄ１の向きと同じ（平行）になっている第２の移動体５の姿勢である。なお、本発明によると、回転させる順序を変えたり、別のオイラー角表現や回転行列などにより、第１の移動体３から見た第２の移動体５の姿勢を表してもよい。この場合、後述の式（２０）も変更する。
【００５３】
この例では、θ１として、第１の方位センサ９は、θ１ｚとθ１ｘｙとを計測する。
θ１ｚは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、ｚ軸回りの周方向に関して、第１の移動体３から見て、第１基準方向Ｄ１（ｘ軸）に対して、第２の移動体５が存在する方位（ラジアン）である。すなわち、θ１ｚは、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３をｘ−ｙ平面に正射影した点Ｐ３ｘｙと３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分の、第１基準方向Ｄ１（ｘ軸）に対する方位である。
θ１ｘｙは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３と前記３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。
【００５４】
θ１ｚが計測されたら、演算部１３は、次の式（２）（３）により、ＸｔとＣｏｖＸｔを更新する。なお、 ＣｏｖＸｔは、Ｘｔの分散共分散行列を示す（以下同様）。
Ｘｔ＝Ｘｔ−Ｋ１（Ｙ１−θ１ｚ） ・・・（２）
ＣｏｖＸｔ＝（Ｉ−Ｋ１Ｂ１）・ＣｏｖＸｔ ・・・（３）
【００５５】
ここで、式（２）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（３）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（３）において、Ｉは、単位行列を示す。
【００５６】
Ｋ１は、カルマンゲインであり、次の式（４）で表わされる。
Ｋ１＝ＣｏｖＸｔ・Ｂ１Ｔ・Ｖ１−１ ・・・（４）
Ｂ１は、次の［数２］の式（５）で表わされる偏微分行列であり、その上付き文字「Ｔ」は転置行列を示す。
【数２】
上式（４）のＶ１は、次の式で表わされ、その上付き数字「-１」は逆行列を示す。
Ｖ１＝Ｂ１・ＣｏｖＸｔ・Ｂ１Ｔ＋Ｒ ・・・（６）
ここで、Ｒは、方位の計測誤差として想定される分散値である（以下同様）。
【００５７】
上式（２）のＹ１は、次の式で表わされる。
Ｙ１＝ｇ１（Ｘｔ）＝ｔａｎ−１（ｙ／ｘ） ・・・（７）
【００５８】
θ１ｘｙが計測されたら、演算部１３は、次の式（８）（９）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ２（Ｙ２−θ１ｘｙ） ・・・（８）
ＣｏｖＸｔ＝（Ｉ−Ｋ２Ｂ２）・ＣｏｖＸｔ ・・・（９）
【００５９】
ここで、式（８）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（９）において、左辺のＣｏｖＸｔは、当該式により更新されたＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（９）において、Ｉは、単位行列を示す。
【００６０】
Ｋ２は、カルマンゲインであり、次の式（１０）で表わされる。
Ｋ２＝ＣｏｖＸｔ・Ｂ２Ｔ・Ｖ２−１ ・・・（１０）
Ｂ２は、次の［数３］の式（１１）で表わされる偏微分行列である。
【数３】
上式（１０）のＶ２は、次の式（１２）で表わされる。
Ｖ２＝Ｂ２・ＣｏｖＸｔ・Ｂ２Ｔ＋Ｒ ・・・（１２）
【００６１】
上式（８）のＹ２は、次の［数４］の式（１３）で表わされる。
【数４】
【００６２】
この例では、θ２として、第２の方位センサ１１は、θ２ｚとθ２ｘｙとを計測する。
θ２ｚは、図５（Ｂ）に示すように、互いに直交するｘ軸、ｙ軸、ｚ軸を有し第２の移動体５に固定された３次元座標系Ｂにおいて、ｚ軸回りの周方向に関して、第２の移動体５から見て、第２基準方向Ｄ２（ｘ軸）に対して、第１の移動体３が存在する方位（ラジアン）である。すなわち、θ２ｚは、３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１をｘ−ｙ平面に正射影した点Ｐ１ｘｙと３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分の、第２基準方向Ｄ２（ｘ軸）に対する方位である。
θ２ｘｙは、図５（Ｂ）に示すように、前記３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１と前記３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。
【００６３】
θ２ｚが計測されたら、演算部１３は、次の式（１４）（１５）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ３（Ｙ３−θ２ｚ） ・・・（１４）
ＣｏｖＸｔ＝（Ｉ−Ｋ３Ｂ３）・ＣｏｖＸｔ ・・・（１５）
【００６４】
ここで、式（１４）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（１５）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（１５）において、Ｉは、単位行列を示す。
【００６５】
Ｋ３は、カルマンゲインであり、次の式（１６）で表わされる。
Ｋ３＝ＣｏｖＸｔ・Ｂ３Ｔ・Ｖ３−１ ・・・（１６）
Ｂ３は、次の［数５］の式（１７）で表わされる偏微分行列である。
【数５】
上式（１６）のＶ３は、次の式（１８）で表わされる。
Ｖ３＝Ｂ３・ＣｏｖＸｔ・Ｂ３Ｔ＋Ｒ ・・・（１８）
【００６６】
上式（１４）のＹ３は、次の式（１９）で表わされる。
Ｙ３＝ｇ３（Ｘｔ）＝ｔａｎ−１（ｙＢ／ｘＢ） ・・・（１９）
ここで、ｘＢとｙＢは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値である。そのため、ｘＢとｙＢは、次の［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。
【数６】
なお、この式（２０）において、ｃとｓは、それぞれ、余弦関数（ｃｏｓ）と正弦関数（ｓｉｎ）を表わす。また、この式（２０）において、ｚＢは、３次元座標系Ｂにおけるｚ軸の座標値である。
【００６７】
θ２ｘｙが計測されたら、演算部１３は、次の式（２１）（２２）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ４（Ｙ４−θ２ｘｙ） ・・・（２１）
ＣｏｖＸｔ＝（Ｉ−Ｋ４Ｂ４）・ＣｏｖＸｔ ・・・（２２）
【００６８】
ここで、式（２１）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（２２）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（２２）において、Ｉは、単位行列を示す。
【００６９】
Ｋ４は、カルマンゲインであり、次の式（２３）で表わされる。
Ｋ４＝ＣｏｖＸｔ・Ｂ４Ｔ・Ｖ４−１ ・・・（２３）
Ｂ４は、次の［数７］の式（２４）で表わされる偏微分行列である。
【数７】
上式（２３）のＶ４は、次の式（２５）で表わされる。
Ｖ４＝Ｂ４・ＣｏｖＸｔ・Ｂ４Ｔ＋Ｒ ・・・（２５）
【００７０】
上式（２１）のＹ４は、次の［数８］の式（２６）で表わされる。
【数８】
この式におけるｘＢとｙＢとｚＢは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値とｚ軸の座標であり、上述した［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。
【００７１】
上述のように更新された内部状態を、前記相対関係（前記相対姿勢）とする。すなわち、更新された内部状態Ｘｔのｒｘ、ｒｙ、ｒｚを前記相対関係とする。
【００７２】
上述の具体例では、３次元座標系Ａと３次元座標系Ｂをそれぞれ第１および第２の移動体３、５に固定していたが、任意の座標系を用いてよい。例えば、静止座標系を用いてもよい。この静止座標系に対して、第１および第２の移動体３、５は移動し姿勢を変更する。この場合、内部状態は、１２の変数からなる。１２の変数は、第１の移動体３の位置を示す３つの変数と、第１の移動体３の姿勢を示す３つの変数と、第２の移動体５の位置を示す３つの変数と、第２の移動体５の姿勢を示す３つの変数とからなる。そこで、好ましくは、上述の第１および第２の方位センサ９、１１が計測した方位に加えて、第１および第２の移動体３、５間の距離の計測値や、静止座標系における移動体３、５の位置の計測値を用いて、カルマンフィルタにより内部状態を更新する。更新された内部状態から前記相対関係を推定する。ただし、移動体３、５間の距離の計測値と、移動体３、５の位置の計測値の一方または両方が得られなくても、カルマンフィルタにより内部状態を更新し、更新された内部状態から前記相対関係を推定できる。
【００７３】
（パーティクルフィルタ）
ベイズフィルタとしてパーティクルフィルタを用いる場合には、次のようにする。
【００７４】
（ａ）まず、第１の移動体３の位置および姿勢と、第２の移動体５の位置および姿勢とを１つのサンプルとして、演算部１３は、第１および第２の移動体３、５が取り得るサンプルを多数生成する。これらのサンプルは、第１および第２の移動体３、５が移動する空間の座標系で表わされてよい。すなわち、この座標系に対して、第１および第２の移動体３、５が移動する。これらのサンプルは、前記方位θ１と方位θ２の一方を計測した時刻のものである。
（ｂ）演算部１３は、多数のサンプルから前記方位θ１と方位θ２の一方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を前記確率分布として取得（更新）し、当該確率に基づいて、多数のサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ１と方位θ２の一方に適合する度合いが高いほど、各サンプルが実際に存在する確率が高いものであってもよいし、前記方位θ１と方位θ２の一方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｃ）次に、演算部１３は、前記（ｂ）の処理で抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。すなわち、前記（ｂ）の処理で抽出した各サンプルについて、当該サンプルである第１および第２の移動体３、５の位置および姿勢が、方位θ１と方位θ２の他方を計測した時にとっていた値を、各移動体３、５の運動モデルに基づいて求め、求めた当該値により、当該他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を予測する。
（ｄ）さらに、演算部１３は、前記（ｂ）の処理で抽出したサンプルから前記方位θ１と方位θ２の他方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ１と方位θ２の他方と、予測した第１および第２の移動体３、５の相対位置と相対姿勢とに基づいて、前記抽出した各サンプルが実際に存在する確率を更新し、当該更新した確率に基づいて、前記抽出したサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ１と方位θ２の他方との適合度が高いサンプルほど、高くなるものであってもよいし、前記方位θ１と方位θ２の他方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｅ）その後、演算部１３は、前記（ｄ）で抽出したサンプルに基づいて、前記相対関係を推定する。当該サンプルの全ては、第１の移動体３に対する第２の移動体５の相対姿勢が等しいものとなっていると考えられる。
【００７５】
方位θ１と方位θ２の一方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｂ）の処理において、当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ１と方位θ２の一方に適合するサンプルを抽出してもよい。
同様に、方位θ１と方位θ２の他方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｄ）の処理において、予測した第１および第２の移動体３、５の前記相対位置に基づいて当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ１と方位θ２の他方に適合するサンプルを抽出してもよい。
なお、第１および第２の移動体３、５間の距離は、例えば、第１または第２の移動体３、５に設置されたレーザ距離計により計測してよい。このレーザ距離計は、この測定時刻またはその直前に得られた方位θ１と方位θ２に相当する方位において、第１および第２の移動体３、５間の距離を計測してよい。
【００７６】
また、前記（ｂ）の処理において、演算部１３は、前記（ｂ）で抽出したサンプルに近いサンプルを新たに生成してもよい。すなわち、演算部１３は、抽出したサンプルと、移動体３、５の位置と姿勢が近いサンプルを新たに生成してもよい。この場合、前記（ｃ）の処理では、演算部１３は、前記新たに生成した各サンプルについても、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。さらに、前記（ｄ）の処理では、演算部１３は、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルとについて予測した相対位置と相対姿勢に基づいて、上述と同様に、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルから、前記方位θ１と方位θ２の他方に適合するサンプルを抽出する。
【００７７】
方位θ１と方位θ２を繰り返し計測し、方位θ１と方位θ２を計測する度に、前記（ａ）を省略して、当該方位θ１と方位θ２について前記（ｂ）（ｃ）（ｄ）の処理を行ってもよい。この場合、方位θ１と方位θ２が再び計測されたら、直前の前記（ｄ）で抽出されたサンプルについて、当該方位θ１と方位θ２の一方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。次いで、再度の前記（ｂ）において、これらの予測した相対位置と相対姿勢に基づいて、上述と同様に、直前の前記（ｄ）で抽出されたサンプルから前記方位θ１と方位θ２の一方に適合するサンプルを抽出する。その後、上述と同様に、前記（ｃ）（ｄ）の処理を行う。なお、前記（ｅ）の処理では、最後に行った前記（ｄ）の処理で抽出されたサンプルに基づいて、前記相対関係を求める。
【００７８】
［移動体の制御］
次に、各移動体の移動制御について説明する。
【００７９】
移動体３、５毎に制御装置が設けられ、該制御装置は、対応する移動体３または５の位置に基づいて、該移動装置が目標位置へ移動するように、該移動体の移動速度と移動方向を制御する。
前記制御装置は、対応する移動体３または５に設置されてよい。移動体３、５は、例えば、走行車輪や歩行脚により路面上を移動する移動装置、空中を移動する飛行体、または、宇宙空間を移動する宇宙機である。
【００８０】
このような制御を行うために、移動体３、５が移動する空間に固定された静止座標系における移動体３、５の、位置を検出する位置検出装置７が設けられる。この位置検出装置７は、適宜の手段で構成されてよいが、図１の例のように、移動体３、５が移動する空間に設置された３次元レーザレーダまたはカメラであってよい。
各移動体３、５の前記制御装置は、位置検出装置７により検出した移動体３、５の位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、対応する移動体３または５の移動速度と移動方向を制御する。
【００８１】
このような移動体の制御方法として、例えば、次の制御方法１、２を採用できる。
【００８２】
（制御方法１）
第２の移動体５に第１の移動体３を追従させる場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第２の移動体５に第１の移動体３が追従して移動するように第１の移動体３の移動速度と移動方向を制御する。一方、第２の移動体５の前記制御装置は、位置検出装置７により検出した第２の移動体５の、位置に基づいて、第２の移動体５が目標位置へ移動するように、第２の移動体５の移動速度と移動方向を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。
【００８３】
（制御方法２）
第１の移動体３と第２の移動体５の衝突を避ける場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第１および第２の移動体３、５が互いに衝突しないように第１の移動体３の移動を制御する。例えば、第１の移動体３の前記制御装置は、前記相対位置と相対姿勢に基づいて、第１および第２の移動装置が互いに衝突する可能性があるかを判断し、この可能性があると判断した場合には、この衝突を避けるように第１の移動体３の移動速度と移動方向の一方または両方を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。
【００８４】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。
【００８５】
上述の実施形態では、各方位センサ９、１１は、カメラと画像処理装置により構成されたが、他の手段で構成されてもよい。例えば、各方位センサ９、１１は、レーザレーダにより構成されてもよい。
【符号の説明】
【００８６】
３ 第１の移動体、５ 第２の移動体、７ 位置検出装置、９ 第１の方位センサ、１１ 第２の方位センサ、１３ 演算部、１０ 相対姿勢計測装置
【技術分野】
【０００１】
本発明は、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法と装置に関する。
【背景技術】
【０００２】
複数の移動体が、互いに独立してエリア内を移動する場合に、複数の移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御などを行うことがある。
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。
【０００３】
上述の制御を行うために、移動体の位置と姿勢を、次の方法Ａ、ＢまたはＣにより計測している。
【０００４】
方法Ａでは、移動体に、速度センサやジャイロセンサなどを設置する。速度センサによる速度計測値を時間積分して、移動体の位置を求め、ジャイロセンサによる角速度計測値を時間積分して、移動体の姿勢を求める。
【０００５】
方法Ｂでは、移動体に、外界センサを搭載する。この外界センサにより、移動体の外部において既知の位置に存在する指標の方向と、該指標までの距離を計測する。この計測結果を、地図データと照合する。この照合に基づいて、移動体の自己位置と姿勢を求める。例えば、外界センサとして３次元レーザレーダやカメラを用いる。なお、下記の特許文献１では、宇宙機である移動体の姿勢として、当該移動体の進行方向と前記指標に相当する地球や太陽との相対関係を検出している。
【０００６】
方法Ｃでは、移動体の外部に設置された計測機器（例えば、カメラ）により移動体を含む画像を取得し、この画像に基づいて移動体の位置と姿勢を求める。
【０００７】
なお、後述する本発明の実施形態で使用可能な技術が、下記の特許文献２〜４に記載されている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特許第２５１４９８７号
【特許文献２】特開２００９−３３３６６号公報
【特許文献３】特開２００７−３２３３号公報
【特許文献４】特開２０１１−２８４１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、移動体の位置と姿勢のうち、姿勢を精度よく計測することは難しい。
【００１０】
上述した方法Ａでは、ジャイロセンサの計測誤差が積分されることにより、移動体の姿勢の計測精度が低下する可能性がある。
【００１１】
上述した方法Ｂでは、３次元レーザレーダやカメラにより指標を認識する必要がある。そのため、指標を設定できない場合には、移動体の姿勢を計測できない。
【００１２】
上述した方法Ｃでは、移動体の姿勢を得るためには、計測機器により、移動体における３つの位置を互いに区別して計測する必要がある。従って、移動体と計測機器との距離が大きくなると、移動体における３つの位置を互いに区別して計測することが困難となる。
【００１３】
一方、２つの移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御を行う場合には、２つの移動体の相対姿勢が得られればよい。すなわち、移動体が移動するエリアに固定された静止座標系から見た各移動体の姿勢を求めることは必ずしも必要ではない。
【００１４】
そこで、本発明の目的は、２つの移動体の相対姿勢を精度よく求めることができる方法と装置を提供することにある。
【課題を解決するための手段】
【００１５】
上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
（Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
（Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
（Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
（Ｄ）計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法が提供される。
【００１６】
本発明の好ましい実施形態によると、前記（Ｂ）を行う時の前記相対姿勢は、前記（Ｃ）を行う時の前記相対姿勢と同じであり、
前記（Ｂ）（Ｃ）で計測された方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との前記相対関係を幾何学的に算出する。
【００１７】
本発明の別の実施形態によると、前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、
更新された確率分布に基づいて、前記相対関係を推定する。
【００１８】
好ましくは、前記移動体の位置および姿勢を内部状態とし、
該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。
【００１９】
好ましい別の例では、前記（Ｄ）において、
第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
前記抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
前記方位θ１と方位θ２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
当該抽出したサンプルに基づいて、前記相対関係を推定する。
【００２０】
また、上記目的を達成するため、本発明によると、第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置が提供される。
【発明の効果】
【００２１】
上述した本発明によると、第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
この方法では、第１の方位センサは、第２の移動体の方位を認識できればよく、第２の移動体における３つの位置を互いに区別して計測する必要がない。第２の方位センサも同様である。
よって、第１の移動体と第２の移動体との距離が大きくても、相対姿勢を高精度に求めることができる。
【図面の簡単な説明】
【００２２】
【図１】本発明の実施形態による相対姿勢計測装置を示す。
【図２】（Ａ）は、第１の移動体に固定した２次元座標系を示し、（Ｂ）は、第２の移動体に固定した２次元座標系を示す。
【図３】本発明の実施形態による相対姿勢計測方法を示すフローチャートである。
【図４】第１の移動体に固定された３次元座標系Ａと、第２の移動体に固定された３次元座標系Ｂを示す。
【図５】（Ａ）は、第１の方位センサが計測する方位θ１ｚ、θ１ｘｙを示し、（Ｂ）は、第２の方位センサが計測する方位θ２ｚ、θ２ｘｙを示す。
【発明を実施するための形態】
【００２３】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２４】
図１は、本発明の実施形態による相対姿勢計測装置１０の構成図である。相対姿勢計測装置１０は、第１の移動体３に対する第２の移動体５の相対姿勢を計測する。
図１の例では、第１および第２の移動体３、５は、路面を走行するものであり、図１は、各移動体３、５を上方から見た平面図である。なお、図１において、各移動体３、５の位置を検出する位置検出装置７（３次元レーザレーダ）が、移動体３、５以外の固定位置に設けられている。
【００２５】
相対姿勢計測装置１０は、第１の方位センサ９と第２の方位センサ１１と演算部１３とを備える。
【００２６】
第１の方位センサ９は、第１の移動体３に設けられる。第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。ここで、第１基準方向Ｄ１とは、第１の移動体３に設定された基点Ｐ１から、該基点Ｐ１と間隔をおいて第１の移動体３に設定された方位点Ｐ２へ向かう方向である。好ましくは、第１基準方向Ｄ１は、第１の移動体３の前後方向であって、第１の移動体３の後方から前方へ向かう方向である。ただし、第１基準方向Ｄ１は、第１の移動体３の前後方向から既知の角度だけ傾いた方向であってもよい。
【００２７】
第１の方位センサ９は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第１基準方向Ｄ１を向くように、または、第１基準方向Ｄ１に対して既知の角度だけ傾いた方向を向くように第１の移動体３に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第２の移動体５との位置関係に基づいて、第２の移動体５が位置する方位θ１を求める。この時、画像処理装置は、第２の移動体５に設けられた指標を、前記画像内において第２の移動体５の位置として特定してよい。なお、好ましくは、第１の移動体３から見てすべての方向の画像が得られるように、全方向の視野を有する１つのカメラを第１の移動体３に設置するか、もしくは、異なる方向を向く複数のカメラを第１の移動体３に設置するのがよい。
【００２８】
ここで、θ１は、例えば、第１の移動体３に固定された２次元座標系Ａで表現されてよい。例えば、２次元座標系Ａは、図２（Ａ）に示すように、その原点が基点Ｐ１であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第１基準方向Ｄ１に相当する。すなわち、ｘ軸上に方位点Ｐ２が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ１は、図２（Ａ）において、第１基準方向Ｄ１（ｘ軸）に対する角度であり、基点Ｐ１を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ１を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。
【００２９】
第２の方位センサ１１は、第２の移動体５に設けられる。第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。ここで、第２基準方向Ｄ２とは、第２の移動体５に設定された基点Ｐ３から、該基点Ｐ３と間隔をおいて第２の移動体５に設定された方位点Ｐ４へ向かう方向である。好ましくは、第２基準方向Ｄ２は、第２の移動体５の前後方向であって、第２の移動体５の後方から前方へ向かう方向である。ただし、第２基準方向Ｄ２は、第２の移動体５の前後方向から既知の角度だけ傾いた方向であってもよい。
【００３０】
第２の方位センサ１１は、例えば、次のように、カメラと画像処理装置とにより構成されている。カメラは、第２基準方向Ｄ２を向くように、または、第２基準方向Ｄ２に対して既知の角度だけ傾いた方向を向くように第２の移動体５に設置される。画像処理装置は、カメラにより撮像した画像において、その中心位置と第１の移動体３との位置関係に基づいて、第１の移動体３が位置する方位θ２を求める。この時、画像処理装置は、第１の移動体３に設けられた指標を、前記画像内において第１の移動体３の位置として特定してよい。なお、好ましくは、第２の移動体５から見てすべての方位の画像が得られるように、全方向の視野を有する１つのカメラを第２の移動体５に設置するか、もしくは、異なる方向を向く複数のカメラを第２の移動体５に設置するのがよい。
【００３１】
ここで、θ２は、例えば、第２の移動体５に固定された２次元座標系Ｂで表現されてよい。例えば、２次元座標系Ｂは、図２（Ｂ）に示すように、その原点が基点Ｐ３であり、互いに直交するｘ軸とｙ軸を有する。このｘ軸は、水平方向を向き、第２基準方向Ｄ２に相当する。すなわち、ｘ軸上に方位点Ｐ４が存在する。ｙ軸は、ｘ軸と直交する水平方向を向く。
θ２は、図２（Ｂ）において、第２基準方向Ｄ２（ｘ軸）に対する角度であり、基点Ｐ３を中心に、反時計回りにずれた方位を正の値とし、基点Ｐ３を中心に、時計回りにずれた方位を負の値として、その最大絶対値がπ（ラジアン）になるようにする。
【００３２】
各移動体３、５に設ける上述の指標として、例えば、次のものを用いてよい。
一例では、指標は、特許文献２のように、特定のパターンで輝度が変化する発光ダイオードである。この場合、方位センサ９、１１は、カメラを用いて、短い設定時間にわたる動画を画像として撮像し、画像処理装置を用いて、画像において、特定のパターンで輝度が変化する画素の位置を、指標の位置として特定する。
別の例では、指標は、特許文献３、４に記載されたような幾何学的模様である。この場合、方位センサ９、１１の画像処理装置は、幾何学的模様と同じ模様を予め記憶しており、カメラを用いて得た画像において、予め記憶した当該模様と同じ模様の部分を、指標の位置として特定する。
【００３３】
上述の指標は、第１および第２の移動体３、５の外面（例えば、外周面）に、間隔をおいて取り付けられていてよい。
【００３４】
演算部１３は、第１の方位センサ９が計測した方位θ１と、第１の方位センサ９が計測した方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係を求める。求めた相対関係が、第１の移動体３に対する第２の移動体５の相対姿勢となる。相対関係は、例えば、図１のように、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度θｔで表わされる。このなす角度θｔは、第１基準方向Ｄ１、または、この方向Ｄ１に平行な方向と、第２基準方向Ｄ２、または、この方向Ｄ２に平行な方向とのなす角度を意味する。
【００３５】
図３は、上述した相対姿勢計測装置１０を用いた相対姿勢計測方法を示すフローチャートである。
【００３６】
ステップＳ１において、第１の移動体３に第１の方位センサ９を設け、第２の移動体５に第２の方位センサ１１を設ける。
ステップＳ２において、第１の方位センサ９は、第１の移動体３から見た、第１基準方向Ｄ１に対する第２の移動体５の方位θ１を計測する。
ステップＳ３において、第２の方位センサ１１は、第２の移動体５から見た、第２基準方向Ｄ２に対する第１の移動体３の方位θ２を計測する。
ステップＳ４において、演算部１３は、計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める。
なお、ステップＳ２、Ｓ３は、移動体３、５が移動している時に行われてよい。
【００３７】
以下、ステップＳ４で行われる演算部１３による演算方法１、２を詳しく説明する。
【００３８】
［演算方法１］
演算方法１は、ステップＳ２を行う時の前記相対姿勢が、ステップＳ３を行う時の前記相対姿勢と同じである場合に採用される。このような場合としては、ステップＳ２を行う時とステップＳ３を行う時との間で、第１および第２の移動体３、５の姿勢と位置が変化していない場合がある。演算方法１を行う他の場合として、ステップＳ２とステップＳ３を同じ時刻に行う場合がある。
【００３９】
演算部１３は、ステップＳ２とステップＳ３でそれぞれ計測した方位θ１と方位θ２とに基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係（なす角度θｔ）を幾何学的演算により算出する。ここでは、上述の図２のように、方位θ１、θ２は、上述の２次元座標系Ａ、Ｂで表現できるものとする。なお、図１において、Ｄ１ｐは、第１基準方向と平行であって基点Ｐ３を通る直線である。
演算部１３は、方位θ１と方位θ２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度をθｔを、次の式（１）により求める。なお、この式（１）において、θ１とθ２とθｔの単位はラジアンである。
θｔ＝θ１−θ２−π ・・・（１）
【００４０】
上述では、第１および第２の移動体３、５が、２次元平面である路面を移動する場合について説明したので、ここでは、方位θ１、θ２は、２次元座標系Ａ、Ｂで表わした。しかし、第１および第２の移動体３、５が、３次元空間内を移動する場合には、演算部１３は、３次元座標系で表わされた方位θ１、θ２に基づいて、第１基準方向Ｄ１と第２基準方向Ｄ２とのなす角度を次のように求める。
【００４１】
図４は、移動体３、５が移動する空間において、これらの移動体３、５にそれぞれ固定された３次元座標系Ａ、Ｂを示す。各３次元座標系Ａ，Ｂは、互いに直交するｘ軸とｙ軸とｚ軸を有する。上述の第１基準方向Ｄ１は、３次元座標系Ａのｘ軸に一致し、上述の第２基準方向Ｄ２は、３次元座標系Ｂのｘ軸に一致する。
【００４２】
図４において、第１の方位センサ９は、上述の方位θ１として方位θ１ｚとθ１ｘｙを計測し、第２の方位センサ１１は、上述の方位θ２として方位θ２ｚとθ２ｘｙを計測する。なお、図４において、Ｐ１ｘｙは、３次元座標系Ａの原点Ｐ１を３次元座標系Ｂのｘ−ｙ平面に正射影した点であり、Ｐ３ｘｙは、３次元座標系Ｂの原点Ｐ３を３次元座標系Ａのｘ−ｙ平面に正射影した点である。
【００４３】
また、第１基準方向Ｄ１をベクトルｖ１で表わし、第１基準方向Ｄ２をベクトルｖ２で表わし、Ｒｔを回転行列として、
ｖ１＝Ｒｔｖ２
が成り立つとする。この場合、第１基準方向Ｄ１と第２基準方向Ｄ２との相対関係は、上記のＲｔで表わすことができる。ここで、Ｒｔは、例えば、
Ｒｔ＝Ｒ１ｚ（π）Ｒ１ｚ（-θ１ｚ）Ｒ１ｘｙ（θ１ｘｙ）Ｒ２ｘｙ（θ２ｘｙ）Ｒ２ｚ（θ２ｚ）
で表わされる。
Ｒ２ｚ（θ２ｚ）は、空間上の各点を、３次元座標系Ｂのｚ軸回りに、θ２ｚだけ回転させる行列である。Ｒ２ｘｙ（θ２ｘｙ）は、３次元座標系ＢをＲ２ｚ（θ２ｚ）により回転させた座標系Ｂ１のｙ軸回りに、空間上の各点を、θ２ｘｙだけ回転させる行列である。Ｒ１ｘｙ（θ１ｘｙ）は、３次元座標系Ａのｚ軸回りに、座標系Ａをθ１ｚだけ回転させた座標系Ａ１のｙ軸回りに、空間上の各点を、θ１ｘｙだけ回転させる行列である。Ｒ１ｚ（−θ１ｚ）は、３次元座標系Ａのｚ軸回りに、空間上の各点を、−θ１ｚだけ回転させる行列である。Ｒ１ｚ（π）は、空間上の各点を、３次元座標系Ａのｚ軸回りに、π（ラジアン）だけ回転させる行列である。なお、各回転角θは、大きさを表しており、θは、各軸の正の方向から見て反時計回りの回転を示し、−θは、各軸の正の方向から見て時計回りの回転を示している。
【００４４】
［演算方法２］
演算方法２は、第１の方位センサ９と第２の方位センサ１１が、異なる時刻に、または、同じ時刻に、上述した方位θ１と方位θ２を計測する場合に採用される。
この場合、演算部１３は、ベイズフィルタ（ベイズの定理）を用いて、前記相対関係を推定する。すなわち、演算部１３は、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、更新された確率分布に基づいて、前記相対関係を推定する。なお、ベイズフィルタを用いる場合に、前記相対関係が一意に求められないときにも、確からしい前記相対関係を、確率分布を利用して求めることができる。この確率分布を絞り込むために、他の計測手段により、第１および第２の移動体３、５の位置や姿勢を計測し、これらの計測値をさらに用いてもよい。
【００４５】
以下、ベイズフィルタとして、カルマンフィルタとパーティクルフィルタを用いた例を順に説明する。
【００４６】
（カルマンフィルタ）
ベイズフィルタとしてカルマンフィルタを用いる場合には、次のようにする。
【００４７】
まず、前記移動体の位置および姿勢を内部状態とする。例えば、第１の移動体３に設定された前記基点Ｐ１と前記方位点Ｐ２に対して静止している座標系における第２の移動体５の位置と姿勢を内部状態とする。すなわち、ここでの例では、内部状態は、第１の移動体３に対する第２の移動体５の相対位置と相対姿勢である。
演算部１３は、前記確率分布を用いたカルマンフィルタにより、第１の方位センサ９が計測した前記方位θ１に基づいて内部状態と該内部状態の分散共分散行列を更新するとともに、第２の方位センサ１１が計測した前記方位θ２に基づいて内部状態とその分散共分散行列を更新し、更新された内部状態（相対姿勢）から前記相対関係を推定し、または、以下で詳しく説明する具体例のように、更新された内部状態を前記相対関係とする。なお、カルマンフィルタを用いる場合には、前記確率分布は、内部状態と分散共分散行列で表わされる。
【００４８】
なお、分散共分散行列が示す内部状態の確からしさが、しきい値以上になるまで、方位θ１と方位θ２の計測を繰り返すのがよい。この場合、演算部１３は、方位θ１が計測される度に、カルマンフィルタにより、当該方位θ１に基づいて内部状態とその分散共分散行列を更新するとともに、方位θ２が計測される度に、カルマンフィルタにより、当該方位θ２に基づいて内部状態とその分散共分散行列を更新する。分散共分散行列が示す内部状態の確からしさが、しきい値以上になったら、演算部１３は、最新の内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする。
【００４９】
以下において、カルマンフィルタを用いた場合の具体例を説明する。
【００５０】
まず、次の［数１］で示すように、内部状態Ｘｔを定義するとともに、内部状態Ｘｔの初期値と、内部状態Ｘｔの初期分散共分散行列ＣｏｖＸｔを定義しておく。
【００５１】
【数１】
【００５２】
［数１］において、ｘ、ｙ、ｚは、現在において、第１の移動体３から見た第２の移動体５の相対位置を示す。すなわち、ｘ、ｙ、ｚは、それぞれ、第１の移動体３に固定された３次元座標系Ａにおける互いに直交するｘ軸とｙ軸とｚ軸の座標値である。この３次元座標系Ａのｘ軸は、第１基準方向Ｄ１に相当する。すなわち、この３次元座標系Ａの原点が基点Ｐ１であり、ｘ軸上に方位点Ｐ２が存在する。
［数１］において、ｒｘ、ｒｙ、ｒｚは、現在において、第１の移動体３から見た第２の移動体５の姿勢を示す角度（ラジアン）である。すなわち、３次元座標系Ａにおいて、第２の移動体５を、ｘ軸と平行であり第２の移動体５（例えば、基点Ｐ３）を通る軸回りにその基準姿勢からｒｘだけ回転させ、次に、ｙ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒｙだけ回転させ、最後に、ｚ軸と平行であり第２の移動体５（基点Ｐ３）を通る軸回りにｒｚだけ回転させると、第１の移動体３から見た現在における第２の移動体５の姿勢となる。基準姿勢とは、例えば、第２基準方向Ｄ２の向きが第１基準方向Ｄ１の向きと同じ（平行）になっている第２の移動体５の姿勢である。なお、本発明によると、回転させる順序を変えたり、別のオイラー角表現や回転行列などにより、第１の移動体３から見た第２の移動体５の姿勢を表してもよい。この場合、後述の式（２０）も変更する。
【００５３】
この例では、θ１として、第１の方位センサ９は、θ１ｚとθ１ｘｙとを計測する。
θ１ｚは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、ｚ軸回りの周方向に関して、第１の移動体３から見て、第１基準方向Ｄ１（ｘ軸）に対して、第２の移動体５が存在する方位（ラジアン）である。すなわち、θ１ｚは、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３をｘ−ｙ平面に正射影した点Ｐ３ｘｙと３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分の、第１基準方向Ｄ１（ｘ軸）に対する方位である。
θ１ｘｙは、図５（Ａ）に示すように、前記３次元座標系Ａにおいて、第２の移動体５の基点Ｐ３と前記３次元座標系Ａの原点（上述の基点Ｐ１）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。
【００５４】
θ１ｚが計測されたら、演算部１３は、次の式（２）（３）により、ＸｔとＣｏｖＸｔを更新する。なお、 ＣｏｖＸｔは、Ｘｔの分散共分散行列を示す（以下同様）。
Ｘｔ＝Ｘｔ−Ｋ１（Ｙ１−θ１ｚ） ・・・（２）
ＣｏｖＸｔ＝（Ｉ−Ｋ１Ｂ１）・ＣｏｖＸｔ ・・・（３）
【００５５】
ここで、式（２）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（３）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（３）において、Ｉは、単位行列を示す。
【００５６】
Ｋ１は、カルマンゲインであり、次の式（４）で表わされる。
Ｋ１＝ＣｏｖＸｔ・Ｂ１Ｔ・Ｖ１−１ ・・・（４）
Ｂ１は、次の［数２］の式（５）で表わされる偏微分行列であり、その上付き文字「Ｔ」は転置行列を示す。
【数２】
上式（４）のＶ１は、次の式で表わされ、その上付き数字「-１」は逆行列を示す。
Ｖ１＝Ｂ１・ＣｏｖＸｔ・Ｂ１Ｔ＋Ｒ ・・・（６）
ここで、Ｒは、方位の計測誤差として想定される分散値である（以下同様）。
【００５７】
上式（２）のＹ１は、次の式で表わされる。
Ｙ１＝ｇ１（Ｘｔ）＝ｔａｎ−１（ｙ／ｘ） ・・・（７）
【００５８】
θ１ｘｙが計測されたら、演算部１３は、次の式（８）（９）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ２（Ｙ２−θ１ｘｙ） ・・・（８）
ＣｏｖＸｔ＝（Ｉ−Ｋ２Ｂ２）・ＣｏｖＸｔ ・・・（９）
【００５９】
ここで、式（８）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（９）において、左辺のＣｏｖＸｔは、当該式により更新されたＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（９）において、Ｉは、単位行列を示す。
【００６０】
Ｋ２は、カルマンゲインであり、次の式（１０）で表わされる。
Ｋ２＝ＣｏｖＸｔ・Ｂ２Ｔ・Ｖ２−１ ・・・（１０）
Ｂ２は、次の［数３］の式（１１）で表わされる偏微分行列である。
【数３】
上式（１０）のＶ２は、次の式（１２）で表わされる。
Ｖ２＝Ｂ２・ＣｏｖＸｔ・Ｂ２Ｔ＋Ｒ ・・・（１２）
【００６１】
上式（８）のＹ２は、次の［数４］の式（１３）で表わされる。
【数４】
【００６２】
この例では、θ２として、第２の方位センサ１１は、θ２ｚとθ２ｘｙとを計測する。
θ２ｚは、図５（Ｂ）に示すように、互いに直交するｘ軸、ｙ軸、ｚ軸を有し第２の移動体５に固定された３次元座標系Ｂにおいて、ｚ軸回りの周方向に関して、第２の移動体５から見て、第２基準方向Ｄ２（ｘ軸）に対して、第１の移動体３が存在する方位（ラジアン）である。すなわち、θ２ｚは、３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１をｘ−ｙ平面に正射影した点Ｐ１ｘｙと３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分の、第２基準方向Ｄ２（ｘ軸）に対する方位である。
θ２ｘｙは、図５（Ｂ）に示すように、前記３次元座標系Ｂにおいて、第１の移動体３の基点Ｐ１と前記３次元座標系Ｂの原点（上述の基点Ｐ３）とを結ぶ線分が、ｘ−ｙ平面となす角度（ラジアン）である。
【００６３】
θ２ｚが計測されたら、演算部１３は、次の式（１４）（１５）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ３（Ｙ３−θ２ｚ） ・・・（１４）
ＣｏｖＸｔ＝（Ｉ−Ｋ３Ｂ３）・ＣｏｖＸｔ ・・・（１５）
【００６４】
ここで、式（１４）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（１５）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（１５）において、Ｉは、単位行列を示す。
【００６５】
Ｋ３は、カルマンゲインであり、次の式（１６）で表わされる。
Ｋ３＝ＣｏｖＸｔ・Ｂ３Ｔ・Ｖ３−１ ・・・（１６）
Ｂ３は、次の［数５］の式（１７）で表わされる偏微分行列である。
【数５】
上式（１６）のＶ３は、次の式（１８）で表わされる。
Ｖ３＝Ｂ３・ＣｏｖＸｔ・Ｂ３Ｔ＋Ｒ ・・・（１８）
【００６６】
上式（１４）のＹ３は、次の式（１９）で表わされる。
Ｙ３＝ｇ３（Ｘｔ）＝ｔａｎ−１（ｙＢ／ｘＢ） ・・・（１９）
ここで、ｘＢとｙＢは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値である。そのため、ｘＢとｙＢは、次の［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。
【数６】
なお、この式（２０）において、ｃとｓは、それぞれ、余弦関数（ｃｏｓ）と正弦関数（ｓｉｎ）を表わす。また、この式（２０）において、ｚＢは、３次元座標系Ｂにおけるｚ軸の座標値である。
【００６７】
θ２ｘｙが計測されたら、演算部１３は、次の式（２１）（２２）により、ＸｔとＣｏｖＸｔを更新する。
Ｘｔ＝Ｘｔ−Ｋ４（Ｙ４−θ２ｘｙ） ・・・（２１）
ＣｏｖＸｔ＝（Ｉ−Ｋ４Ｂ４）・ＣｏｖＸｔ ・・・（２２）
【００６８】
ここで、式（２１）において、左辺のＸｔは、当該式により更新されたＸｔであり、右辺のＸｔは、当該式によりＸｔを更新する直前のＸｔである。また、式（２２）において、左辺のＣｏｖＸｔは、当該式により更新されたＣｏｖＸｔであり、右辺のＣｏｖＸｔは、当該式によりＣｏｖＸｔを更新する直前のＣｏｖＸｔである。また、式（２２）において、Ｉは、単位行列を示す。
【００６９】
Ｋ４は、カルマンゲインであり、次の式（２３）で表わされる。
Ｋ４＝ＣｏｖＸｔ・Ｂ４Ｔ・Ｖ４−１ ・・・（２３）
Ｂ４は、次の［数７］の式（２４）で表わされる偏微分行列である。
【数７】
上式（２３）のＶ４は、次の式（２５）で表わされる。
Ｖ４＝Ｂ４・ＣｏｖＸｔ・Ｂ４Ｔ＋Ｒ ・・・（２５）
【００７０】
上式（２１）のＹ４は、次の［数８］の式（２６）で表わされる。
【数８】
この式におけるｘＢとｙＢとｚＢは、それぞれ、３次元座標系Ｂにおけるｘ軸の座標値とｙ軸の座標値とｚ軸の座標であり、上述した［数６］の式（２０）により、第１の移動体３に固定された３次元座標系Ａの座標値ｘ、ｙ、ｚで表わされる。
【００７１】
上述のように更新された内部状態を、前記相対関係（前記相対姿勢）とする。すなわち、更新された内部状態Ｘｔのｒｘ、ｒｙ、ｒｚを前記相対関係とする。
【００７２】
上述の具体例では、３次元座標系Ａと３次元座標系Ｂをそれぞれ第１および第２の移動体３、５に固定していたが、任意の座標系を用いてよい。例えば、静止座標系を用いてもよい。この静止座標系に対して、第１および第２の移動体３、５は移動し姿勢を変更する。この場合、内部状態は、１２の変数からなる。１２の変数は、第１の移動体３の位置を示す３つの変数と、第１の移動体３の姿勢を示す３つの変数と、第２の移動体５の位置を示す３つの変数と、第２の移動体５の姿勢を示す３つの変数とからなる。そこで、好ましくは、上述の第１および第２の方位センサ９、１１が計測した方位に加えて、第１および第２の移動体３、５間の距離の計測値や、静止座標系における移動体３、５の位置の計測値を用いて、カルマンフィルタにより内部状態を更新する。更新された内部状態から前記相対関係を推定する。ただし、移動体３、５間の距離の計測値と、移動体３、５の位置の計測値の一方または両方が得られなくても、カルマンフィルタにより内部状態を更新し、更新された内部状態から前記相対関係を推定できる。
【００７３】
（パーティクルフィルタ）
ベイズフィルタとしてパーティクルフィルタを用いる場合には、次のようにする。
【００７４】
（ａ）まず、第１の移動体３の位置および姿勢と、第２の移動体５の位置および姿勢とを１つのサンプルとして、演算部１３は、第１および第２の移動体３、５が取り得るサンプルを多数生成する。これらのサンプルは、第１および第２の移動体３、５が移動する空間の座標系で表わされてよい。すなわち、この座標系に対して、第１および第２の移動体３、５が移動する。これらのサンプルは、前記方位θ１と方位θ２の一方を計測した時刻のものである。
（ｂ）演算部１３は、多数のサンプルから前記方位θ１と方位θ２の一方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を前記確率分布として取得（更新）し、当該確率に基づいて、多数のサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ１と方位θ２の一方に適合する度合いが高いほど、各サンプルが実際に存在する確率が高いものであってもよいし、前記方位θ１と方位θ２の一方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｃ）次に、演算部１３は、前記（ｂ）の処理で抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。すなわち、前記（ｂ）の処理で抽出した各サンプルについて、当該サンプルである第１および第２の移動体３、５の位置および姿勢が、方位θ１と方位θ２の他方を計測した時にとっていた値を、各移動体３、５の運動モデルに基づいて求め、求めた当該値により、当該他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を予測する。
（ｄ）さらに、演算部１３は、前記（ｂ）の処理で抽出したサンプルから前記方位θ１と方位θ２の他方に適合するサンプルを抽出する。すなわち、演算部１３は、前記方位θ１と方位θ２の他方と、予測した第１および第２の移動体３、５の相対位置と相対姿勢とに基づいて、前記抽出した各サンプルが実際に存在する確率を更新し、当該更新した確率に基づいて、前記抽出したサンプルから、当該確率がしきい値以上となるサンプルを抽出する。この確率は、前記方位θ１と方位θ２の他方との適合度が高いサンプルほど、高くなるものであってもよいし、前記方位θ１と方位θ２の他方に適合するサンプルでは１であり、そうでないサンプルでは０であってもよい。
（ｅ）その後、演算部１３は、前記（ｄ）で抽出したサンプルに基づいて、前記相対関係を推定する。当該サンプルの全ては、第１の移動体３に対する第２の移動体５の相対姿勢が等しいものとなっていると考えられる。
【００７５】
方位θ１と方位θ２の一方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｂ）の処理において、当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ１と方位θ２の一方に適合するサンプルを抽出してもよい。
同様に、方位θ１と方位θ２の他方を計測する時刻またはその直後に、第１および第２の移動体３、５間の距離を計測し、演算部１３は、前記（ｄ）の処理において、予測した第１および第２の移動体３、５の前記相対位置に基づいて当該距離に適合するサンプルを選択しておき、選択されたサンプルの中から、前記方位θ１と方位θ２の他方に適合するサンプルを抽出してもよい。
なお、第１および第２の移動体３、５間の距離は、例えば、第１または第２の移動体３、５に設置されたレーザ距離計により計測してよい。このレーザ距離計は、この測定時刻またはその直前に得られた方位θ１と方位θ２に相当する方位において、第１および第２の移動体３、５間の距離を計測してよい。
【００７６】
また、前記（ｂ）の処理において、演算部１３は、前記（ｂ）で抽出したサンプルに近いサンプルを新たに生成してもよい。すなわち、演算部１３は、抽出したサンプルと、移動体３、５の位置と姿勢が近いサンプルを新たに生成してもよい。この場合、前記（ｃ）の処理では、演算部１３は、前記新たに生成した各サンプルについても、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。さらに、前記（ｄ）の処理では、演算部１３は、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルとについて予測した相対位置と相対姿勢に基づいて、上述と同様に、前記新たに生成した各サンプルと前記（ｂ）で抽出したサンプルから、前記方位θ１と方位θ２の他方に適合するサンプルを抽出する。
【００７７】
方位θ１と方位θ２を繰り返し計測し、方位θ１と方位θ２を計測する度に、前記（ａ）を省略して、当該方位θ１と方位θ２について前記（ｂ）（ｃ）（ｄ）の処理を行ってもよい。この場合、方位θ１と方位θ２が再び計測されたら、直前の前記（ｄ）で抽出されたサンプルについて、当該方位θ１と方位θ２の一方を計測した時における第１および第２の移動体３、５の相対位置と相対姿勢を、各移動体３、５の運動モデルに基づいて予測する。次いで、再度の前記（ｂ）において、これらの予測した相対位置と相対姿勢に基づいて、上述と同様に、直前の前記（ｄ）で抽出されたサンプルから前記方位θ１と方位θ２の一方に適合するサンプルを抽出する。その後、上述と同様に、前記（ｃ）（ｄ）の処理を行う。なお、前記（ｅ）の処理では、最後に行った前記（ｄ）の処理で抽出されたサンプルに基づいて、前記相対関係を求める。
【００７８】
［移動体の制御］
次に、各移動体の移動制御について説明する。
【００７９】
移動体３、５毎に制御装置が設けられ、該制御装置は、対応する移動体３または５の位置に基づいて、該移動装置が目標位置へ移動するように、該移動体の移動速度と移動方向を制御する。
前記制御装置は、対応する移動体３または５に設置されてよい。移動体３、５は、例えば、走行車輪や歩行脚により路面上を移動する移動装置、空中を移動する飛行体、または、宇宙空間を移動する宇宙機である。
【００８０】
このような制御を行うために、移動体３、５が移動する空間に固定された静止座標系における移動体３、５の、位置を検出する位置検出装置７が設けられる。この位置検出装置７は、適宜の手段で構成されてよいが、図１の例のように、移動体３、５が移動する空間に設置された３次元レーザレーダまたはカメラであってよい。
各移動体３、５の前記制御装置は、位置検出装置７により検出した移動体３、５の位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、対応する移動体３または５の移動速度と移動方向を制御する。
【００８１】
このような移動体の制御方法として、例えば、次の制御方法１、２を採用できる。
【００８２】
（制御方法１）
第２の移動体５に第１の移動体３を追従させる場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第２の移動体５に第１の移動体３が追従して移動するように第１の移動体３の移動速度と移動方向を制御する。一方、第２の移動体５の前記制御装置は、位置検出装置７により検出した第２の移動体５の、位置に基づいて、第２の移動体５が目標位置へ移動するように、第２の移動体５の移動速度と移動方向を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。
【００８３】
（制御方法２）
第１の移動体３と第２の移動体５の衝突を避ける場合には、次のように制御を行う。第１の移動体３の前記制御装置は、位置検出装置７により検出した各移動体３、５の位置から第１の移動体３に対する第２の移動体５の相対位置を求め、この相対位置と、相対姿勢計測装置１０により求めた相対姿勢に基づいて、第１および第２の移動体３、５が互いに衝突しないように第１の移動体３の移動を制御する。例えば、第１の移動体３の前記制御装置は、前記相対位置と相対姿勢に基づいて、第１および第２の移動装置が互いに衝突する可能性があるかを判断し、この可能性があると判断した場合には、この衝突を避けるように第１の移動体３の移動速度と移動方向の一方または両方を制御する。
この場合、相対姿勢計測装置１０の演算部１３は、第１の移動体３に設置されてよく、第２の方位センサ１１が計測した前記方位θ２を第２の方位センサ１１から無線により受け、求めた相対姿勢を、第１の移動体３の前記制御装置に出力する。
【００８４】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。
【００８５】
上述の実施形態では、各方位センサ９、１１は、カメラと画像処理装置により構成されたが、他の手段で構成されてもよい。例えば、各方位センサ９、１１は、レーザレーダにより構成されてもよい。
【符号の説明】
【００８６】
３ 第１の移動体、５ 第２の移動体、７ 位置検出装置、９ 第１の方位センサ、１１ 第２の方位センサ、１３ 演算部、１０ 相対姿勢計測装置
【特許請求の範囲】
【請求項１】
第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
（Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
（Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
（Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
（Ｄ）計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法。
【請求項２】
前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、
更新された確率分布に基づいて、前記相対関係を推定する、ことを特徴とする請求項１に記載の移動体の相対姿勢計測方法。
【請求項３】
前記移動体の位置および姿勢を内部状態とし、
該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
【請求項４】
前記（Ｄ）において、
第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
前記抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
前記方位θ１と方位θ２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
当該抽出したサンプルに基づいて、前記相対関係を推定する、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
【請求項５】
第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置。
【請求項１】
第１および第２の移動体の相対姿勢を計測する相対姿勢計測方法であって、
（Ａ）第１の移動体に第１の方位センサを設け、第２の移動体に第２の方位センサを設け、
（Ｂ）第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
（Ｃ）第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
（Ｄ）計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める、ことを特徴とする移動体の相対姿勢計測方法。
【請求項２】
前記（Ｄ）において、前記移動体の位置および姿勢の確率分布を、前記方位θ１と方位θ２に基づいて更新し、
更新された確率分布に基づいて、前記相対関係を推定する、ことを特徴とする請求項１に記載の移動体の相対姿勢計測方法。
【請求項３】
前記移動体の位置および姿勢を内部状態とし、
該内部状態の初期値を設定するとともに、該内部状態の分散共分散行列の初期値を設定し、
前記（Ｄ）において、前記確率分布を用いたカルマンフィルタにより、前記（Ｂ）により得た前記方位θ１に基づいて前記内部状態と前記分散共分散行列を更新するとともに、前記（Ｃ）により得た前記方位θ２に基づいて前記内部状態と前記分散共分散行列を更新し、更新された内部状態から前記相対関係を推定し、または、更新された内部状態を前記相対関係とする、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
【請求項４】
前記（Ｄ）において、
第１の移動体の位置および姿勢と、第２の移動体の位置および姿勢とを１つのサンプルとして、第１および第２の移動体が取り得るサンプルを多数生成し、
前記方位θ１と方位θ２の一方に基づいて、各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記多数のサンプルから抽出し、
前記抽出した各サンプルについて、前記方位θ１と方位θ２の他方を計測した時における第１および第２の移動体の相対位置と相対姿勢を、各移動体の運動モデルに基づいて予測し、
前記方位θ１と方位θ２の他方と、前記予測した第１および第２の移動体の相対位置と相対姿勢とに基づいて、前記抽出された各サンプルが実際に存在する確率を更新し、更新した当該確率がしきい値以上となるサンプルを、前記抽出されたサンプルから抽出し、
当該抽出したサンプルに基づいて、前記相対関係を推定する、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。
【請求項５】
第１および第２の移動体の相対姿勢を計測する相対姿勢計測装置であって、
第１の移動体に設けられる第１の方位センサと、第２の移動体に設けられる第２の方位センサと、を備え、
第１の移動体に設定された基点から、該基点と間隔をおいて第１の移動体に設定された方位点へ向かう方向を第１基準方向として、第１の方位センサは、第１の移動体から見た、第１基準方向に対する第２の移動体の方位θ１を計測し、
第２の移動体に設定された基点から、該基点と間隔をおいて第２の移動体に設定された方位点へ向かう方向を第２基準方向として、第２の方位センサは、第２の移動体から見た、第２基準方向に対する第１の移動体の方位θ２を計測し、
計測した方位θ１と方位θ２とに基づいて、第１基準方向と第２基準方向との相対関係を前記相対姿勢として求める演算部をさらに備える、ことを特徴とする移動体の相対姿勢計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１３−６１３０７（Ｐ２０１３−６１３０７Ａ）
【公開日】平成２５年４月４日（２０１３．４．４）
【国際特許分類】
【出願番号】特願２０１１−２０１４５８（Ｐ２０１１−２０１４５８）
【出願日】平成２３年９月１５日（２０１１．９．１５）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【公開日】平成２５年４月４日（２０１３．４．４）
【国際特許分類】
【出願日】平成２３年９月１５日（２０１１．９．１５）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
[ Back to top ]