特許 5939395 移動体の相対姿勢計測方法と装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5939395

(24)【登録日】2016年5月27日

(45)【発行日】2016年6月22日

(54)【発明の名称】移動体の相対姿勢計測方法と装置

(51)【国際特許分類】

   G05D 1/02 20060101AFI20160609BHJP

【ＦＩ】

   G05D1/02 Z

【請求項の数】7

【全頁数】15

(21)【出願番号】特願2012-204256(P2012-204256)

(22)【出願日】2012年9月18日

(65)【公開番号】特開2014-59719(P2014-59719A)

(43)【公開日】2014年4月3日

【審査請求日】2015年7月22日

(73)【特許権者】

【識別番号】000000099

【氏名又は名称】株式会社ＩＨＩ

(74)【代理人】

【識別番号】100097515

【弁理士】

【氏名又は名称】堀田 実

(74)【代理人】

【識別番号】100136700

【弁理士】

【氏名又は名称】野村 俊博

(72)【発明者】

【氏名】江本 周平

(72)【発明者】

【氏名】坂野 肇

(72)【発明者】

【氏名】曽根原 光治

【審査官】 川東 孝至

(56)【参考文献】

【文献】 特開平６−３１４１２４（ＪＰ，Ａ）

【文献】 特開平１−１１２１８３（ＪＰ，Ａ）

【文献】 特開２０１１−１５０４４３（ＪＰ，Ａ）

【文献】 特開２００９−１５７４３０（ＪＰ，Ａ）

【文献】 特開２００８−３０４２６８（ＪＰ，Ａ）

【文献】 特開２００５−１７３９６８（ＪＰ，Ａ）

【文献】 特開２００９−２２３６３４（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｄ １／０２

Ｇ０１Ｃ １５／００

(57)【特許請求の範囲】

【請求項1】

対象物の方位を計測可能な方位センサを有する２以上の移動体と、
各移動体から方位を計測可能な１以上の被計測体と、
各移動体と通信可能な姿勢演算装置と、を備え、
（Ａ）姿勢演算装置により、複数の移動体から第１移動体と第２移動体を選択し、
（Ｂ）第１移動体により第２移動体の方位を計測し、
（Ｃ）第２移動体により第１移動体の方位を計測し、
（Ｄ）第１移動体と第２移動体により同一の被計測体又は位置関係が既知である複数の被計測体の方位をそれぞれ計測し、
（Ｅ）姿勢演算装置により、計測した複数の方位から第１移動体と第２移動体の相対姿勢を演算する、ことを特徴とする移動体の相対姿勢計測方法。

【請求項2】

前記（Ｅ）において、移動体及び被計測体の位置と姿勢を内部状態とし、ベイズフィルタによって、前記相対姿勢を求める、ことを特徴とする請求項１に記載の移動体の相対姿勢計測方法。

【請求項3】

前記（Ｅ）において、
（Ｅ１）移動体及び被計測体の内部状態の初期値と初期分布を設定し、
（Ｅ２）前記複数の方位を計測した観測時刻における移動体及び被計測体の内部状態を予測し、
（Ｅ３）前記複数の方位を用いて、予測した内部状態を修正する、ことを特徴とする請求項２に記載の移動体の相対姿勢計測方法。

【請求項4】

前記（Ｅ）において、前記相対姿勢を幾何学的に求める、ことを特徴とする請求項１に記載の移動体の相対姿勢計測方法。

【請求項5】

前記各方位は、それぞれ方位角と仰角である、ことを特徴とする請求項１乃至４のいずれか１項に記載の移動体の相対姿勢計測方法。

【請求項6】

移動体が３以上の場合に、そのうちの１つを被計測体とする、ことを特徴とする請求項１乃至５のいずれか１項に記載の移動体の相対姿勢計測方法。

【請求項7】

対象物の方位を計測可能な方位センサを有する２以上の移動体と、
各移動体から方位を計測可能な１以上の被計測体と、
各移動体と通信可能な姿勢演算装置と、を備え、
（Ａ）姿勢演算装置により、複数の移動体から第１移動体と第２移動体を選択し、
（Ｂ）第１移動体により第２移動体の方位を計測し、
（Ｃ）第２移動体により第１移動体の方位を計測し、
（Ｄ）第１移動体と第２移動体により同一の被計測体又は位置関係が既知である複数の被計測体の方位をそれぞれ計測し、
（Ｅ）姿勢演算装置により、計測した複数の方位から第１移動体と第２移動体の相対姿勢を演算する、ことを特徴とする移動体の相対姿勢計測装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、複数の移動体の相対姿勢を計測する相対姿勢計測方法と装置に関する。

【背景技術】

【0002】

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

【0003】

上述の制御を行うために、従来は移動体の位置と姿勢を以下のＡ〜Ｄの方法により計測している。

【0004】

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

【0005】

方法Ｂでは、移動体に、カメラ、レーザセンサなどの外界センサを搭載する。この外界センサにより、移動体の外部において既知の位置に存在する指標の方向と、該指標までの距離を計測する。この計測結果を、地図データと照合する。この照合に基づいて、移動体の自己位置と姿勢を求める。
例えば、特許文献１では、移動体の自己姿勢を計測する姿勢センサとして、スターセンサ、地球センサ、太陽センサなどを用いている。
また特許文献２は、ランドマークの配置を示す環境地図をあらかじめ用意できない場合のために、移動体の自己位置と姿勢を推定すると同時に、環境地図を自動作成する方法を開示している。

【0006】

方法Ｃでは、移動体の外部にある機器と連携して方向や距離を計測し、自己位置と姿勢を求める。例えばＧＰＳシステムが該当する。ＧＰＳシステムでは、３以上の衛星との距離から位置を求める、或いはＧＰＳを使って車体の３ケ所で位置を求めて姿勢を算出することができる。
方法Ｄでは、移動体の外部に設置された外界センサ（例えば、カメラ）により移動体を含む画像を取得し、この画像に基づいて移動体の位置と姿勢を求める。方法Ｄの例としては、特許文献２に、カメラ画像上の特徴点と対象物の３次元形状データから対象物の位置と姿勢を算出する方法が示されている。

【0007】

また、特許文献３は、２つの環境地図間の姿勢ずれを補正することで、一方の環境地図上の点を他方の環境地図の座標系に合わせて座標変換する方法を開示している。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特許第２５１４９８７号公報、「対象物の位置推定装置」

【特許文献2】特開２００８−３０４２６８号公報、「情報処理装置、および情報処理方法、並びにコンピュータ・プログラム」

【特許文献3】特開２００９−１５７４３０号公報、「座標補正方法、座標補正プログラム、及び自律移動ロボット」

【発明の概要】

【発明が解決しようとする課題】

【0009】

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

【0010】

上述した方法Ａでは、姿勢の絶対量を計測することができない。従って、エンコーダやジャイロセンサの計測精度が低いと、角速度の誤差が累積され、精度よく姿勢を計測できない。

【0011】

上述した方法Ｂでは、カメラやレーザセンサでランドマークを認識する必要がある。そのため、ランドマークとなる特徴的な物体が周囲にない環境では移動体の姿勢を計測できない。

【0012】

上述した方法Ｃ、Ｄで姿勢を計測する場合、物体上の異なる点の位置を３点以上計測し、各点の位置関係から姿勢を計測する。そのためセンサと移動体の距離が遠いと、姿勢を計測することが困難になる。例えば、カメラを使う場合、距離が離れると画像上の移動体像が小さくなり特徴点が複数得られなくなる。

【0013】

一方、移動体を制御したり操縦したりする際に必要となるのは、移動体と対象物との「相対的な位置と姿勢」である。
例えば、エリア内の特定の物体に移動体を追従させる場合や、複数の移動体を衝突しないように制御する場合は、追従対象や他の移動体との相対的な位置と姿勢が得られれば良い。

【0014】

しかし、移動体と対象物の双方に方位センサを設置し、両センサの計測結果から相対姿勢を求める場合、３次元の姿勢を対象とすると解が一意に求まらない問題ある。例えば、図１に示すように、両方の移動体が互いに正対している場合、両者の進行方向に対するロール角α１、α２は方位センサからは求まらない。

【0015】

本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、誤差が累積されることなく複数の移動体の相対姿勢を精度よく求めることができ、３次元の姿勢を対象とすることができ、特徴的な物体が周囲にない環境でも適用できる移動体の相対姿勢計測方法と装置を提供することにある。

【課題を解決するための手段】

【0016】

本発明によれば、対象物の方位を計測可能な方位センサを有する２以上の移動体と、
各移動体から方位を計測可能な１以上の被計測体と、
各移動体と通信可能な姿勢演算装置と、を備え、
（Ａ）姿勢演算装置により、複数の移動体から第１移動体と第２移動体を選択し、
（Ｂ）第１移動体により第２移動体の方位を計測し、
（Ｃ）第２移動体により第１移動体の方位を計測し、
（Ｄ）第１移動体と第２移動体により同一の被計測体又は位置関係が既知である複数の被計測体の方位をそれぞれ計測し、
（Ｅ）姿勢演算装置により、計測した複数の方位から第１移動体と第２移動体の相対姿勢を演算する、ことを特徴とする移動体の相対姿勢計測方法が提供される。

【0017】

また本発明によれば、対象物の方位を計測可能な方位センサを有する２以上の移動体と、
各移動体から方位を計測可能な１以上の被計測体と、
各移動体と通信可能な姿勢演算装置と、を備え、
（Ａ）姿勢演算装置により、複数の移動体から第１移動体と第２移動体を選択し、
（Ｂ）第１移動体により第２移動体の方位を計測し、
（Ｃ）第２移動体により第１移動体の方位を計測し、
（Ｄ）第１移動体と第２移動体により同一の被計測体又は位置関係が既知である複数の被計測体の方位をそれぞれ計測し、
（Ｅ）姿勢演算装置により、計測した複数の方位から第１移動体と第２移動体の相対姿勢を演算する、ことを特徴とする移動体の相対姿勢計測装置が提供される。

【発明の効果】

【0018】

上記本発明の方法と装置によれば、第１移動体により第２移動体の方位（方位角と仰角）を計測し、第２移動体により第１移動体の方位を計測し、第１移動体と第２移動体により同一の被計測体の方位をそれぞれ計測するので、姿勢演算装置により第１移動体、第２移動体、及び被計測体の３点の方位計測結果を統合することで、第１移動体と第２移動体の相対姿勢を一意に求められる。

【0019】

また、同一の被計測体の代わりに、位置関係が既知である複数の被計測体の方位を計測する場合も、３点の方位計測結果の組み合わせとして、同様に複数の移動体の相対姿勢を求めることができる。

【0020】

方位の計測は、物体間の距離（センサと対象物の距離）が離れていても精度が落ちない。従って、上記方法では、計測した複数の方位のみに基づくので、誤差が累積されることなく複数の移動体の相対姿勢を精度よく求めることができる。

【0021】

また、方位として方位角と仰角を計測することにより、３次元の姿勢を対象とすることができる。

【0022】

また、移動体が３以上の場合に、そのうちの１つを被計測体とすることにより、特徴的な物体が周囲にない環境でも適用できる。

【図面の簡単な説明】

【0023】

【図1】２台の移動体が正対している例を示す図である。

【図2】本発明の使用例を示す図である。

【図3】２台の移動体Ａ、Ｂの座標系の定義を示す図である。

【図4】２台の移動体Ａ、Ｂから１台の被計測体Ｃを計測する場合の座標系の定義を示す図である。

【図5】式（１）における４つの回転行列の説明図である。

【図6】式（１）における３つの回転行列の説明図である。

【図7】αａ、αｂを求める方法の説明図である。

【図8】同一の物体を計測しない例を示す図である。

【発明を実施するための形態】

【0024】

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

【0025】

図２は、本発明の使用例を示す図である。
この図に示すように、本発明の相対姿勢計測装置は、対象物の方位を計測可能な方位センサを有する２以上の移動体Ａ、Ｂと、各移動体Ａ、Ｂから方位を計測可能な１以上の被計測体Ｃと、各移動体Ａ、Ｂと通信可能な姿勢演算装置１０とを備える。

【0026】

移動体Ａ、Ｂには方位センサ１２が搭載されており、周囲の物体のある方位を方位角と仰角として計測することができる。なお方位角と仰角は一般的な極座標表現と同じ定義である。方位センサ１２の例としては、カメラやレーザーレーダが挙げられる。移動体Ａ、Ｂ及び被計測体Ｃには、カメラなどで検出するためのマーカーを設置するのが好ましい。
なお、この例では、被計測体Ｃにも方位センサ１２が搭載されているが、これは必須ではなく、省略することができる。

【0027】

この例では、エリア内に複数の移動体Ａ、Ｂ（例えば移動ロボット）や１以上の被計測体Ｃ（例えば固定デバイス）を配置し、各移動体Ａ、Ｂ、被計測体Ｃの位置と姿勢を求める。得られた各点の位置と姿勢は、移動体Ａ、Ｂの制御やエリアの地図作成、監視などに用いる。

【0028】

図３は、２台の移動体Ａ、Ｂの座標系の定義を示す図である。
この図では、移動体Ａ、Ｂの正面をｘ軸とする座標系Ａ、Ｂを定義している。なおこの図で点ａは、座標系Ａのｘｙ平面への移動体Ｂの正射影である。また点ｂは、座標系Ｂのｘｙ平面への移動体Ａの正射影である。

【0029】

本発明では、（Ａ）姿勢演算装置１０により、複数の移動体Ａ、Ｂから第１移動体Ａと第２移動体Ｂを選択する。移動体が３台以上の場合も同様である。
次に、図３において、本発明では（Ｂ）第１移動体Ａにより、第１移動体Ａから見た第２移動体Ｂの方位を方位角θ_ａｂ、仰角φ_ａｂとして計測する。
また、（Ｃ）第２移動体Ｂにより、第２移動体Ｂから見た第１移動体Ａの方位を方位角θ_ｂａ、仰角φ_ｂａとして計測する。

【0030】

図４は、２台の移動体Ａ、Ｂから１台の被計測体Ｃを計測する場合の座標系の定義を示す図である。なおこの図で点ｃは、座標系ＡのＸＹ平面への被計測体Ｃの正射影である。また点ｄは、座標系ＢのＸＹ平面への被計測体Ｃの正射影である。
この図において、本発明ではさらに、（Ｄ）第１移動体Ａと第２移動体Ｂにより、同一の被計測体Ｃの方位をそれぞれ計測する。すなわち、第１移動体Ａにより、第１移動体Ａから見た被計測体Ｃの方位を方位角θ_ａｃ、仰角φ_ａｃとして計測する。また、第２移動体Ｂにより、第２移動体Ｂから見た被計測体Ｃの方位を方位角θ_ｂｃ、仰角φ_ｂｃとして計測する。
なお後述するように、被計測体Ｃは、位置関係が既知である複数の被計測体であってもよい（図９参照）。

【0031】

図２〜図４において、姿勢演算装置１０は、例えば通信機能を有するコンピュータであり、幾何学的方法又はベイズフィルタによる方法により、計測した複数の方位から第１移動体Ａと第２移動体Ｂの相対姿勢を演算するようになっている。

【0032】

以下、幾何学的方法（統合方法１）とベイズフィルタによる方法（統合方法２）を説明する。なお以下の説明では、移動体Ａ、Ｂと被計測体Ｃを単に「物体」と呼ぶ。

【0033】

［幾何学的方法（統合方法１）］
幾何学的方法では、前記（Ｅ）において、第１移動体Ａと第２移動体Ｂの相対姿勢を幾何学的に求める。

【0034】

物体ＡとＢの相対姿勢を、以下のような行列Ｒで定義する。
座標系Ａのｘ、ｙ、ｚ軸の基底ベクトルをそれぞれｅＸ_ａ、ｅＹ_ａ、ｅＺ_ａ、座標系Ｂのｘ、ｙ、ｚ軸の基底ベクトルをそれぞれｅＸ_ｂ、ｅＹ_ｂ、ｅＺ_ｂとすると、［数１］の式（１ａ）となる行列Ｒが相対的な姿勢を表している。従って、Ｒを使って座標系Ｂから座標系Ａを求めることができる。
また行列Ｒは、幾何学的な関係から、［数１］の式（１）で求めることができる。

【0035】

【数1】

【0036】

図５は、式（１）における４つの回転行列の説明図である。

【0037】

Ｒ_ＢＺ（θ_ｂａ）は、座標系Ｂのｚ軸回りに、θ_ｂａだけ回転させる回転行列である（図５（Ａ）参照）。
Ｒ_Ｂ１Ｙ（φ_ｂａ）は、座標系ＢをＲ_ＢＺ（θ_ｂａ）で回転した座標系Ｂ１のｙ軸回りに、φ_ｂａだけ回転させる回転行列である（図５（Ｂ）参照）。
Ｒ_Ｂ２Ｘ（−αｂ）は、座標系Ｂ１をＲ_Ｂ１Ｙ（φ_ｂａ）で回転した座標系Ｂ２のｘ軸回りに、−αｂだけ回転させる回転行列である（図５（Ｃ）参照）。
Ｒ_Ｂ３Ｚ（π）は、座標系Ｂ２をＲ_Ｂ２Ｘ（−α_ｂ）で回転した座標系Ｂ３のｚ軸回りに、πだけ回転させる回転行列である（図５（Ｄ）参照）。

【0038】

図６は式（１）における３つの回転行列の説明図である。
Ｒ_Ａ３Ｘ（αａ）は、座標系Ｂ３をＲ_Ｂ３Ｚ（π）で回転した座標系Ａ３のｘ軸回りに、αａだけ回転させる回転行列である（図６（Ａ）参照）。
Ｒ_Ａ２Ｙ（φ_ａｂ）は、座標系Ａ３をＲ_Ａ３Ｘ（αａ）で回転した座標系Ａ２のｙ軸回りに、φ_ａｂだけ回転させる回転行列である（図６（Ｂ）参照）。
Ｒ_Ａ１Ｚ（−θ_ａｂ）は、座標系Ａ２をＲ_Ａ２Ｙ（φ_ａｂ）で回転した座標系Ａ１のｚ軸回りに、−θ_ａｂだけ回転させる回転行列である（図６（Ｃ）参照）。

【0039】

図７は、αａ、αｂを求める方法の説明図である。
幾何学的方法（統合方法１）では、物体Ｃの方位計測結果を使うことで、以下のようにαａ、αｂを求める。

【0040】

図７（Ａ）は、座標系Ａ２をｘ軸負側から見た図である。
この図において、αａは原点と物体Ｃを結ぶ直線が、ｙ軸となす角度である。従って、［数２］の式（２）で求められる。
またＣｚ_Ａ２、Ｃｙ_Ａ２は、座標系Ａ２で表した物体Ｃのｚ座標値、ｙ座標値なので、［数２］の式（２ａ）で求められる。

【0041】

【数2】

ここで_Ａ２Ｒ_Ａは、座標系Ａから座標系Ａ２に変換する行列、ｒ_ａｃは、物体ＡＣ間の距離である。
_Ａ２Ｒ_Ａは、θ_ａｂ、φ_ａｂから算出する。また、式（２）で除算されるため、ｒ_ａｃは求めなくて良い。

【0042】

αｂは、αａと同様に図７（Ｂ）の関係から［数３］の式（３）（３ａ）で計算する。

【0043】

【数3】

ここで_Ｂ２Ｒ_Ｂは、座標系Ｂから座標系Ｂ２に変換する行列、ｒ_ｂｃは、物体ＢＣ間の距離である。なおｒ_ｂｃは、求めなくて良い。

【0044】

以上の式（２）、（３）を、式（１）に代入することで、行列Ｒが一意に求まる。
上述したように幾何学的方法では、姿勢演算装置１０により第１移動体Ａ、第２移動体Ｂ、及び被計測体Ｃの３点の方位計測結果を統合することで、第１移動体Ａと第２移動体Ｂの相対姿勢を幾何学的に一意に求めることができる。

【0045】

［ベイズフィルタによる方法（統合方法２）］
ベイズフィルタは、各物体（Ａ、Ｂ、Ｃ、・・・）の位置と姿勢としてあり得る範囲を、確率分布として事前に予測し、計測した方位に基づいて、分布を絞り込む方法である。

【0046】

上述した幾何学的方法にて、（θ_ａｂ、φ_ａｂ）、（θ_ｂａ、φ_ｂａ）、（θ_ａｃ、φ_ａｃ）、（θ_ｂｃ、φ_ｂｃ）が決まると、物体Ａ、Ｂ間の三次元相対姿勢が一意の値に決定されることを示した。従って、上記の計測した方位を使えば、ベイズフィルタの確率分布を一意に絞り込むことができる。

【0047】

ベイズフィルタによる方法では、前記（Ｅ）において、移動体Ａ、Ｂ及び被計測体Ｃの位置と姿勢を内部状態とし、ベイズフィルタによって、前記相対姿勢を求める。
すなわち、前記（Ｅ）において、
（Ｅ１）移動体及び被計測体の内部状態の初期値と初期分布を設定し、
（Ｅ２）前記複数の方位を計測した観測時刻における移動体及び被計測体の内部状態を予測し、
（Ｅ３）前記複数の方位を用いて、予測した内部状態を修正する。
以下、ベイズフィルタによる方法を説明する。

【0048】

［数４］の式（４ａ）（４ｂ）にベイズフィルタの例を示す。なお、以下、観測値（計測した方位）を２次元の縦ベクトルＹで表す。ここでＴは転置行列を意味する。

【0049】

【数4】

【0050】

（パーティクルフィルタによる方法）
（１）サンプル作成
物体Ａ、Ｂ、Ｃの位置と姿勢としてあり得る仮説を複数のサンプルとして作成する。サンプル数は、仮説のあり得る範囲に応じて適宜設定する（例：２０００サンプル）。
（２）予測
計測した方位Ｙが得られたとき、Ｙを計測した時刻における物体Ａ、Ｂ、Ｃの位置・姿勢を予測する。例えば、物体Ｂが移動ロボットの場合、移動によって相対関係が変化するため、移動ロボットの運動モデルを使って、計測時刻の位置と姿勢を予測する。
（３）リサンプリング
予測したサンプルのうち、計測した方位Ｙと適合しているサンプルのみを抽出する。
上記（２）、（３）を計測した方位Ｙが得られるごとに繰り返す。計測した方位Ｙとしては幾何学的方法で用いた４種類（方位角θ_ａｂ、仰角φ_ａｂ）（方位角θ_ｂａ、仰角φ_ｂａ）（方位角θ_ａｃ、仰角φ_ａｃ）（方位角θ_ｂｃ、仰角φ_ｂｃ）があり、計測、処理の順番は、どの順でも良い。

【0051】

ベイズフィルタによる方法は、計測時刻における相対関係を予測する予測（２）が含まれるため、Ｙ_ａｂ、Ｙ_ｂａ、Ｙ_ａｃ、Ｙ_ｂｃの計測時刻が異なり、その間に各物体が移動していても、姿勢を推定できる。
リサンプリング（３）にてサンプルを抽出する際、計測した方位Ｙと完全に一致する必要はない。パーティクルフィルタのリサンプリング（３）では、計測した方位に近い順に数サンプル選択し、選択したサンプルに近いサンプルを新たに追加するなどの方法が一般的である。
上記４つの計測した方位Ｙで絞りこむだけでは、ＡＢ間の相対位置（ＡＢ間の距離）が定まらないが、例えば、距離センサを使って、ＡＢ間の距離を計測してサンプルを絞り込めば、相対位置も求められる。

【0052】

（カルマンフィルタによる方法）
カルマンフィルタは、各物体の位置と姿勢を内部状態変数として一般化し、内部状態がとりうる分布を平均値、分散・共分散で表す。基本的な手順はパーティクルフィルタと同じである。
以下に、３つ以上の物体の位置と姿勢を推定する方法を示す。

【0053】

（１）初期値定義
［数５］の式（５ａ）（５ｂ）（５ｃ）のように、内部状態の初期値、初期分散を定義する。内部状態Ｘｉは、物体ｉの位置・姿勢である。初期条件では物体ｉの相対位置は不明なので、適当な位置に設定し、初期分散は可能な限り大きな値とする。また、物体ｉが移動体の場合は、速度や加速度なども内部状態に含めることができる。

【0054】

【数5】

【0055】

推定したい物体全ての内部状態、分散行列をまとめたものを、［数６］の式（６ａ）（６ｂ）のように定義する。

【0056】

【数6】

【0057】

ＣｏｖＸの非対角項は、異なる物体間の内部状態の共分散であり、初期値が不明な場合は、０とする。
（２）内部状態Ｘの予測
方位Ｙを計測した時刻における全体の内部状態Ｘを予測する。この予測処理は、物体ごとに定義した運動モデルを用いて、Ｘの各成分（Ｘｉ）の変化をシミュレーションする。また、分散行列も同様に、運動モデルを用いて予測する。
（３）方位の予測
（２）で予測した内部状態、分散行列から、方位の予測値Ｙ^＊を［数７］の式（７ａ）（７ｂ）で求める。ここで、物体ｉから見たｊの方位が得られた時である。

【0058】

【数7】

【0059】

また、Ｙ^＊の分散行列Ｖを［数８］の式（８ａ）（８ｂ）（８ｃ）で求める。
Ｃ_ｎ^ｂｉは、全体座標系から物体ｉのローカル座標系に変換する行列で、物体ｉの内部状態の姿勢成分から算出する。

【0060】

【数8】

【0061】

（４）修正
計測した方位Ｙと、方位の予測値Ｙ^＊から、内部状態、分散行列を修正する。
なお、以下［数９］の式（９ａ）（９ｂ）（９ｃ）では、観測時刻をｔ＋Δｔとした符号が追加されている。

【0062】

【数9】

【0063】

ここで、Ｂ_ｉｊは、方位Ｙ_ｉｊを得るための、ロボット群全体の計測行列であり、例えばｉ＝１、ｊ＝２の場合、Ｂ_ｉｊ＝（Ｂ_ｉ Ｂ_ｊ ０ ０ ０）、ｉ＝３、ｊ＝２の場合、Ｂ_ｉｊ＝（０ Ｂ_ｊ Ｂ_ｉ ０ ０）である。

【0064】

計測した方位が得られるごとに、（２）の内部状態予測処理、（３）の方位予測処理、（４）の修正処理を繰り返す。

【0065】

図８は、同一の物体を計測しない例を示す図である。
上述した実施形態では、２つの方位センサが同一の物体Ｃを計測しているが、必ずしも、同一の物体を計測する必要はない。例えば、図８のように、物体Ａが物体Ｃを計測、物体Ｂが物体Ｄを計測する場合でも、物体Ｃ−Ｄ間の位置関係が分かっていれば、物体Ａ−Ｂ間の相対姿勢も一意に求まる。従ってこの例では、物体Ｃ−Ｄ−Ｅの三者について本発明を適用すればよい。

【0066】

ここで、「物体Ｃ−Ｄ間の位置関係」とは、例えば、物体Ｂが物体Ｄを計測したときに、物体Ｂから見た物体Ｃの方向がわかる情報を意味する。
すなわち（ベクトルＢ→Ｃ）＝（ベクトルＢ→Ｄ）＋（ベクトルＤ→Ｃ）の関係から、「物体Ｃ−Ｄ間の位置関係」である（ベクトルＤ→Ｃ）が既知であれば、（ベクトルＢ→Ｄ）から（ベクトルＢ→Ｃ）を求めることができる。
言い換えれば、（ベクトルＤ→Ｃ）がわかれば、同一の物体を計測したときと同じく、式（２）（３）のαａ、αｂを求めることができる。

【0067】

なお、逆に、（ベクトルＡ→Ｄ）＝（ベクトルＡ→Ｃ）＋（ベクトルＣ→Ｄ）の関係から、物体Ａから見た物体Ｄの方向を求める際に、「物体Ｃ−Ｄ間の位置関係」である（ベクトルＣ→Ｄ）が既知であれば、（ベクトルＡ→Ｃ）から（ベクトルＡ→Ｄ）を求めることができる。

【0068】

ベイズフィルタの例では、「同一座標系で表した各物体の位置」を求めており、これが上述の（ベクトルＤ→Ｃ）や（ベクトルＣ→Ｄ）に相当する情報となる。

【0069】

物体Ｃとしては、「Ａ、Ｂ以外の移動ロボット」、「エリアに設置したマーカー」、「画像処理などで抽出される、特徴的な形状」などが考えられる。
また、方位センサ１２を搭載しない物体（幾何学的方法での物体Ｃ）については、姿勢が求まらないが、物体Ｃにも方位センサを搭載し、他の物体を計測することで、本発明を適用して姿勢を求められる。

【0070】

なお、上述した（Ｂ）（Ｃ）（Ｄ）の順序は任意であり、例えばセンサ計測処理が終わった順に処理するのが好ましい。また、移動体が３台以上の場合でも、処理する順序は任意である。

【0071】

［幾何学的方法］
移動体が３台以上の場合の幾何学的方法は、以下の順に処理することが好ましい。
センサ計測処理が終わった順に、（Ａ）の選択をして処理する。
４つの方位がすべて得られたときに、（Ｅ）の計算をする。
（１）移動体１、２上のセンサで４つの方位が得られる（順不同）。
（２）移動体１を第１移動体、移動体２を第２移動体として処理する。
（３）移動体２、３上で４つの方位が得られる。
（４）移動体２を第１移動体、移動体３を第２移動体として処理する。

【0072】

［ベイズフィルタによる方法］
移動体が３台以上の場合のベイズフィルタによる方法は、以下の順に処理することが好ましい。
方位が１つ得られるごとにＥ１〜Ｅ３まで処理する。Ｂ→Ｅ → Ｃ→Ｅ → Ｄ→Ｅのような処理順となる。
（１）移動体１が移動体２の方位を計測する。
（２）移動体１を第１移動体、移動体２を第２移動体としたベイズフィルタによる処理を行う。
（３）移動体２が移動体３の方位を計測する。
（４）移動体２を第１移動体、移動体３を第２移動体としたベイズフィルタによる処理を行う。
（５）移動体２が被計測体４の方位を計測する。
（６）（２）の続きとして、移動体１を第１移動体、移動体２を第２移動体としたベイズフィルタによる処理を行う。
（７）（４）の続きとして、移動体２を第１移動体、移動体３を第２移動体としたベイズフィルタによる処理を行う。

【0073】

ベイズフィルタの例は、（１）→（２）→（５）→（６）と（３）→（４）→（５）→（７）は別の処理と解することができる。

【0074】

上記本発明の方法と装置によれば、第１移動体により第２移動体の方位（方位角と仰角）を計測し、第２移動体により第１移動体の方位を計測し、第１移動体と第２移動体により同一の被計測体の方位をそれぞれ計測するので、姿勢演算装置により第１移動体、第２移動体、及び被計測体の３点の方位計測結果を統合することで、第１移動体と第２移動体の相対姿勢を一意に求められる。

【0075】

また、同一の被計測体の代わりに、位置関係が既知である複数の被計測体の方位を計測する場合も、３点の方位計測結果の組み合わせとして、同様に複数の移動体の相対姿勢を求めることができる。

【0076】

方位の計測は、物体間の距離（センサと対象物の距離）が離れていても精度が落ちない。従って、上記方法では、計測した複数の方位のみに基づくので、誤差が累積されることなく複数の移動体の相対姿勢を精度よく求めることができる。

【0077】

また、方位として方位角と仰角を計測することにより、３次元の姿勢を対象とすることができる。

【0078】

また、移動体が３以上の場合に、そのうちの１つを被計測体とすることにより、特徴的な物体が周囲にない環境でも適用できる。

【0079】

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

【符号の説明】

【0080】

Ａ、Ｂ 移動体（物体）、Ｃ 被計測体（物体）、
Ｄ、Ｅ 移動体（物体）、
１０ 姿勢演算装置、
１２ 方位センサ（カメラ、レーザーレーダ）、

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】