(57)【特許請求の範囲】
【請求項１】
複数の対象物体を含むシステムの状態を求める状態データ更新装置であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ設けられる複数の演算通信装置を備え、
各演算通信装置は、
該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測することにより、該一方または両方の観測値を取得する観測部と、
過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定する状態推定部と、
該状態推定部により推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新する状態更新部と、
該状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、該演算通信装置が設けられた対象物体とは別の対象物体から更新データを受ける通信部と、
前記状態更新部により更新した最新の状態データと、前記通信部が受けた更新データとを統合することにより、新たな最新の状態データを求める状態統合部と、を備え、
各演算通信装置において、前記観測部による前記観測値の取得と、前記状態推定部による前記状態データ及び前記誤差データの推定と、前記状態更新部による前記状態データ及び前記誤差データの更新と、前記通信部による前記更新データの送信とが繰り返され、
前記状態データは、前記システムの状態を直接的に表す状態ベクトルから変換された情報ベクトルであり、前記更新データは、更新された情報ベクトルと推定された情報ベクトルとの差に関する情報ベクトル差分量を含み、
各演算通信装置は、今回の前記更新を行う時に、前回の前記更新の時から前記今回の更新の時までに、別の演算通信装置の通信部から複数の前記情報ベクトル差分量を受信した場合、当該複数の前記情報ベクトル差分量と、前記今回の更新で得た前記情報ベクトルとを足し合わせることにより、更新された新たな最新の情報ベクトルを求める、ことを特徴とする状態データ更新装置。
【請求項２】
複数の対象物体を含むシステムの状態を求める状態データ更新装置であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ設けられる複数の演算通信装置を備え、
各演算通信装置は、
該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測することにより、該一方または両方の観測値を取得する観測部と、
過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定する状態推定部と、
該状態推定部により推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新する状態更新部と、
該状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、該演算通信装置が設けられた対象物体とは別の対象物体から更新データを受ける通信部と、
前記状態更新部により更新した最新の状態データと、前記通信部が受けた更新データとを統合することにより、新たな最新の状態データを求める状態統合部と、を備え、
各演算通信装置は、当該演算通信装置が設けられた対象物体の観測部が観測した別の対象物体の位置または速度に基づいて、当該別の対象物体が静止していると判断した場合に、当該別の対象物体の位置または速度の観測値を、当該別の対象物体または他の別の対象物体へ送信する頻度を下げる、ことを特徴とする状態データ更新装置。
【請求項３】
前記状態データは、前記システムの状態を直接的に表す状態ベクトルであり、前記誤差データは、前記状態ベクトルの誤差共分散行列であり、各演算通信装置は、カルマンフィルタを用いて状態ベクトルと誤差共分散行列を更新するように構成されており、
各演算通信装置の状態推定部は、前記推定式に従って、前記観測値を取得した観測時点における状態ベクトルとその誤差共分散行列を推定し、
各演算通信装置の前記状態更新部は、カルマンフィルタを用いて、前記システムの状態ベクトルとその誤差共分散行列を更新し、
各演算通信装置の前記通信部は、前記状態更新部により更新された状態ベクトルを、前記更新データとして、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
各演算通信装置の前記状態統合部は、該演算通信装置により更新した状態ベクトルと、前記通信部が受けた状態ベクトルとにそれぞれ、予め定めた重み係数を乗じて得たベクトル同士を足し合わせることにより、統合した新たな最新の状態ベクトルを求める、ことを特徴とする請求項２に記載の状態データ更新装置。
【請求項４】
前記誤差データは、前記状態ベクトルの誤差共分散行列から変換された情報行列であり、
各演算通信装置の状態推定部は、前記推定式に従って、前記観測値を取得した観測時点における情報ベクトルと情報行列を推定し、
各演算通信装置の状態更新部は、状態推定部により推定された情報ベクトルと情報行列とに基づいて、情報フィルタを用いて、情報ベクトルと情報行列を更新するとともに、更新された情報ベクトルと推定された情報ベクトルとの差に関する情報ベクトル差分量を求め、更新された情報行列と推定された情報行列との差に関する情報行列差分量を求め、
各演算通信装置の通信部は、該演算通信装置の状態更新部が求めた情報ベクトル差分量と情報行列差分量を、前記更新データとして、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
各演算通信装置の前記状態統合部は、
該演算通信装置により更新した情報ベクトルと、前記通信部が受けた情報ベクトル差分量とを足し合わせることにより、統合した新たな最新の情報ベクトルを求めるとともに、該演算通信装置により更新した情報行列と、前記通信部が受けた情報行列差分量とを足し合わせることにより、統合した新たな最新の情報行列を求める、ことを特徴とする請求項１に記載の状態データ更新装置。
【請求項５】
各演算通信装置は、
（Ａ）前記観測部により前記観測値を取得し、
（Ｂ）前記状態推定部により、該観測値が取得された観測時点における情報ベクトルと情報行列を推定し、
（Ｃ）前記状態更新部により、情報ベクトルと情報行列を更新するとともに、情報ベクトル差分量と情報行列差分量を求め、
前記（Ａ）〜（Ｃ）を繰り返し行い、
前記（Ｃ）を行った時に、該（Ｃ）を今回の更新処理とし、該今回に対する前回の前記（Ｃ）を前回の更新処理として、前回の更新処理が行われた時点から今回の更新処理が行われた時点までの期間において、該演算通信装置の通信部が別の演算通信装置から受けたすべての複数の情報ベクトル差分量と今回の更新処理で更新された情報ベクトルとを足し合わせることにより、更新された新たな最新の情報ベクトルを求めるとともに、該期間において、該演算通信装置の通信部が別の演算通信装置から受けたすべての複数の情報行列差分量と今回の更新処理で更新された情報行列とを足し合わせることにより、更新された新たな最新の情報行列を求める、ことを特徴とする請求項４に記載の状態データ更新装置。
【請求項６】
前記演算通信装置は、自身の前記状態更新部による更新に関する前記更新データを送信する送信タイミングを指定し、または、該更新データの送信先とする別の対象物体を選択する、ことを特徴とする請求項１、４又は５に記載の状態データ更新装置。
【請求項７】
複数の演算通信装置は、少なくとも、第１、第２および第３の演算通信装置を含み、
第１の演算通信装置の通信部と第２の演算通信装置の通信部とが、互いに直接通信できない場合に、
第１の演算通信装置の通信部は、第１の演算通信装置の状態更新部による更新に関する更新データを、第３の演算通信装置の通信部を介して、第２の演算通信装置の通信部へ送信可能である、ことを特徴とする請求項１〜６のいずれか一項に記載の状態データ更新装置。
【請求項８】
複数の対象物体を含むシステムの状態を求める状態データ更新方法であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ複数の演算通信装置を設け、
各演算通信装置において、
（Ａ）観測部により、該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測して、該一方または両方の観測値を取得し、
（Ｂ）状態推定部により、過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定し、
（Ｃ）状態更新部により、前記（Ｂ）で推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新し、
（Ｄ）通信部により、前記状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
（Ｅ）該演算通信装置が設けられた対象物体とは別の対象物体から更新データを前記通信部により受けた場合に、状態統合部により、前記（Ｃ）で更新された最新の状態データと、前記通信部により受けた更新データとを統合することにより、新たな最新の状態データを求め、
前記状態データは、前記システムの状態を直接的に表す状態ベクトルから変換された情報ベクトルであり、前記更新データは、更新された情報ベクトルと推定された情報ベクトルとの差に関する情報ベクトル差分量を含み、
各演算通信装置において、
前記（Ａ）〜（Ｃ）を繰り返し行い、
前記（Ｃ）を行った時に、該（Ｃ）を今回の更新処理とし、該今回に対する前回の前記（Ｃ）を前回の更新処理として、前回の更新処理が行われた時点から今回の更新処理が行われた時点までの期間において、別の演算通信装置の通信部から複数の前記情報ベクトル差分量を受信した場合、当該複数の前記情報ベクトル差分量と、前記今回の更新で得た前記情報ベクトルとを足し合わせることにより、更新された新たな最新の情報ベクトルを求める、ことを特徴とする状態データ更新方法。
【請求項９】
複数の対象物体を含むシステムの状態を求める状態データ更新方法であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ複数の演算通信装置を設け、
各演算通信装置において、
（Ａ）観測部により、該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測して、該一方または両方の観測値を取得し、
（Ｂ）状態推定部により、過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定し、
（Ｃ）状態更新部により、前記（Ｂ）で推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新し、
（Ｄ）通信部により、前記状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
（Ｅ）該演算通信装置が設けられた対象物体とは別の対象物体から更新データを前記通信部により受けた場合に、状態統合部により、前記（Ｃ）で更新された最新の状態データと、前記通信部により受けた更新データとを統合することにより、新たな最新の状態データを求め、
当該演算通信装置が設けられた対象物体の観測部が観測した別の対象物体の位置または速度に基づいて、当該別の対象物体が静止していると判断した場合に、当該別の対象物体の位置または速度の観測値を、当該別の対象物体または他の別の対象物体へ送信する頻度を下げる、ことを特徴とする状態データ更新方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の対象物体（例えば、移動ロボット）を含むシステムの状態を求める状態データ更新装置と方法に関する。
【背景技術】
【０００２】
従来において、対象物体の位置や姿勢や速度などの状態データを取得するために、対象物体に設けたセンサ、または、既知の位置に設けたセンサにより、対象物体を観測し、得られた観測値に基づいて、対象物体の状態を表わす状態データを推定している。対象物体は、例えば、移動ロボットまたは物品などである。このような技術は、下記の特許文献１、非特許文献１、非特許文献２などに記載されている。
【０００３】
特許文献１では、移動する複数の物品を対象として、次のように、複数の物品に関する状態データ（位置）を取得している。複数の物品を撮影できるように配置されたカメラにより、複数の物品の位置を観測し、得られた観測値に基づいて、複数の物品の位置を示す状態データを推定して更新している。
【０００４】
非特許文献１では、単体の移動ロボットを対象として、次のように、移動ロボットに関する状態データ（位置や姿勢）を取得している。移動ロボットから、周囲の環境中のランドマーク（例えば目印となる人工物）の位置を観測し、得られた観測値に基づいて、移動ロボットの位置や姿勢を示す状態データを推定して更新している。この時、移動ロボットの位置推定と、移動ロボット近傍の地図の作成とが同時に行われている。位置推定と地図作成を同時に行うことを、ＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ  ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  ｍａｐｐｉｎｇ）という。なお、得られた状態データと地図のデータは、単体の移動ロボットの動作の制御に用いられる。
【０００５】
非特許文献２では、複数の移動ロボットを対象として、次のように、複数の移動ロボットに関する状態データ（位置や姿勢）を取得している。各移動ロボットから、周囲の環境中のランドマーク（例えば目印となる人工物）の位置と、他の移動ロボットの位置を観測し、これらの観測値は、１つの中央演算装置に集約される。集約された観測値に基づいて、１つの中央演算装置が、各移動ロボットの位置や姿勢を示す状態データを推定して更新している。この時、上述したＳＬＡＭが行われている。
【０００６】
なお、下記の非特許文献３には、後述する本発明の実施形態に関連する先行技術（情報ベクトルと情報行列）が記載されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第４８７５２２８号公報
【非特許文献】
【０００８】
【非特許文献１】Ａ．  Ｃｏｓｔａ，  Ｇ．  Ｋａｎｔｏｒ，  Ｈ．  Ｃｈｏｓｅｔ，  “Ｂｅａｒｉｎｇ−ｏｎｌｙ  Ｌａｎｄｍａｒｋ  Ｉｎｉｔｉａｌｉｚａｔｉｏｎ  ｗｉｔｈ  Ｕｎｋｎｏｗｎ  Ｄａｔａ  Ａｓｓｏｃｉａｔｉｏｎ”，  ｉｎ  ２００４  ＩＥＥＥ  Ｉｎｔｌ．  Ｃｏｎｆ．  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，２００４年４月、ｐｐ．１７６４−１７７０
【非特許文献２】大谷、滑川、“マルチロボットの協調自己位置推定と環境認識問題とその収束性解析”、第５４回自動制御連合講演会、２０１１年１１月１９日、２０日
【非特許文献３】スランら著、「確率ロボティクス」、毎日コミュニケーションズ、２００７
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかし、特許文献１や非特許文献１では、単体のカメラや移動ロボットから周囲の環境を観測しているので、算出される状態データの精度には限界がある。
【００１０】
また、非特許文献２では、１つの中央演算装置が集約された観測値を処理するので、対象物体（移動ロボット）の数が多いと、中央演算装置の計算負荷が過大になる。
【００１１】
そこで、本発明の目的は、複数の対象物体を含むシステムの状態データを求める場合に、状態データの推定精度を向上させることができるとともに、計算負荷が１箇所に集中することを避けることができる装置と方法を提供することにある。
【課題を解決するための手段】
【００１２】
上述の目的を達成するため、本発明によると、複数の対象物体を含むシステムの状態を求める状態データ更新装置であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ設けられる複数の演算通信装置を備え、
各演算通信装置は、
該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測することにより、該一方または両方の観測値を取得する観測部と、
過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定する状態推定部と、
該状態推定部により推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新する状態更新部と、
該状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、該演算通信装置が設けられた対象物体とは別の対象物体から更新データを受ける通信部と、
前記状態更新部により更新した最新の状態データと、前記通信部が受けた更新データとを統合することにより、新たな最新の状態データを求める状態統合部と、を備える、ことを特徴とする状態データ更新装置が提供される。
【００１３】
本発明の状態データ更新装置は、例えば、以下のように構成される。
【００１４】
カルマンフィルタを用いる場合には、前記状態データは、前記システムの状態を直接的に表す状態ベクトルであり、前記誤差データは、前記状態ベクトルの誤差共分散行列であり、各演算通信装置は、カルマンフィルタを用いて状態ベクトルと誤差共分散行列を更新するように構成されており、
各演算通信装置の状態推定部は、前記推定式に従って、前記観測値を取得した観測時点における状態ベクトルとその誤差共分散行列を推定し、
各演算通信装置の前記状態更新部は、カルマンフィルタを用いて、前記システムの状態ベクトルとその誤差共分散行列を更新し、
各演算通信装置の前記通信部は、前記状態更新部により更新された状態ベクトルを、前記更新データとして、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
各演算通信装置の前記状態統合部は、該演算通信装置により更新した状態ベクトルと、前記通信部が受けた状態ベクトルとにそれぞれ、予め定めた重み係数を乗じて得たベクトル同士を足し合わせることにより、統合した新たな最新の状態ベクトルを求める。
【００１５】
カルマンフィルタを用いる場合には、重み係数を予め定め、これらの重み係数を、それぞれ、更新した状態ベクトルと、通信部が受けた状態ベクトルに乗算し、これにより得たベクトル同士を足し合わせることにより、更新した状態ベクトルと通信部が受けた状態ベクトルとを統合できる。
【００１６】
情報フィルタを用いる場合には、前記状態データは、前記システムの状態を直接的に表す状態ベクトルから変換された情報ベクトルであり、前記誤差データは、前記状態ベクトルの誤差共分散行列から変換された情報行列であり、
各演算通信装置の状態推定部は、前記推定式に従って、前記観測値を取得した観測時点における情報ベクトルと情報行列を推定し、
各演算通信装置の状態更新部は、状態推定部により推定された情報ベクトルと情報行列とに基づいて、情報フィルタを用いて、情報ベクトルと情報行列を更新するとともに、更新された情報ベクトルと推定された情報ベクトルとの差に関する情報ベクトル差分量を求め、更新された情報行列と推定された情報行列との差に関する情報行列差分量を求め、
各演算通信装置の通信部は、該演算通信装置の状態更新部が求めた情報ベクトル差分量と情報行列差分量を、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
各演算通信装置の前記状態統合部は、
該演算通信装置により更新した情報ベクトルと、前記通信部が受けた情報ベクトル差分量とを足し合わせることにより、統合した新たな最新の情報ベクトルを求めるとともに、
該演算通信装置により更新した情報行列と、前記通信部が受けた情報行列差分量とを足し合わせることにより、統合した新たな最新の情報行列を求める。
【００１７】
情報フィルタを用いる場合には、他の対象物体において更新時に求められた情報行列差分量を受け、この情報行列差分量を、自身の対象物体において更新された情報行列に加算するという簡単な処理により、他の対象物体からの情報行列差分量と、自身の対象物体において更新された情報行列とを統合できる。情報ベクトルについても同様である。
【００１８】
各演算通信装置は、
（Ａ）前記観測部により前記観測値を取得し、
（Ｂ）前記状態推定部により、該観測値が取得された観測時点における情報ベクトルと情報行列を推定し、
（Ｃ）前記状態更新部により、情報ベクトルと情報行列を更新するとともに、情報ベクトル差分量と情報行列差分量を求め、
前記（Ａ）〜（Ｃ）を繰り返し行い、
前記（Ｃ）を行った時に、該（Ｃ）を今回の更新処理とし、該今回に対する前回の前記（Ｃ）を前回の更新処理として、前回の更新処理が行われた時点から今回の更新処理が行われた時点までの期間において、該演算通信装置の通信部が別の演算通信装置から受けたすべての複数の情報ベクトル差分量と今回の更新処理で更新された情報ベクトルとを足し合わせることにより、更新された新たな最新の情報ベクトルを求めるとともに、該期間において、該演算通信装置の通信部が別の演算通信装置から受けたすべての複数の情報行列差分量と今回の更新処理で更新された情報行列とを足し合わせることにより、更新された新たな最新の情報行列を求める。
【００１９】
このように、演算通信装置は、前回の更新処理が行われた時点から今回の更新処理が行われた時点までの期間において、別の演算通信装置から受けたすべての情報ベクトル差分量と今回の更新処理で求めた情報ベクトルとを足し合わる１回の処理で、別の演算通信装置から受けた複数の情報ベクトル差分量を自身で更新した情報ベクトルに統合させることができる。
情報行列についても同様に、１回の処理で、別の演算通信装置から受けた複数の情報行列差分量を自身で更新した情報行列に統合させることができる。
【００２０】
前記演算通信装置は、自身の前記状態更新部による更新に関する前記更新データを送信する送信タイミングを指定し、または、該更新データの送信先とする別の対象物体を選択する。
【００２１】
複数の演算通信装置は、少なくとも、第１、第２、および第３の演算通信装置を含み、
第１の演算通信装置の通信部と第２の演算通信装置の通信部とが、互いに直接通信できない場合に、
第１の演算通信装置の通信部は、第１の演算通信装置の状態更新部による更新に関する更新データを、第３の演算通信装置の通信部を介して、第２の演算通信装置の通信部へ送信可能である。
【００２２】
このように、第１の演算通信装置の通信部と第２の演算通信装置の通信部とは、互いに直接通信できなくても、第３の演算通信装置の通信部を介して、第１の演算通信装置から第２の演算通信装置へ更新データを伝えることができる。
よって、複数の演算通信装置の各々が、全ての他の演算通信装置と通信できなくても、各演算通信装置が求めた更新データを、他の全ての演算通信装置における状態データの統合に用いることが可能となる。
【００２３】
また、上述の目的を達成するため、本発明によると、複数の対象物体を含むシステムの状態を求める状態データ更新方法であって、
前記対象物体は、地上に静止している静止体であるか、または、地上に対して移動する移動体であり、
前記複数の対象物体にそれぞれ複数の演算通信装置を設け、
各演算通信装置において、
（Ａ）観測部により、該演算通信装置が設けられた対象物体の内部状態と該対象物体に対する外部状態の一方または両方を観測して、該一方または両方の観測値を取得し、
（Ｂ）状態推定部により、過去の時点における前記システムの状態データを新たな時点の状態データへ遷移させる推定式に従って、前記観測値を取得した観測時点における前記システムの状態データと、この状態データの誤差を表わす誤差データを推定し、
（Ｃ）状態更新部により、前記（Ｂ）で推定された前記状態データと前記観測値とに基づいて、前記システムの状態データを更新するとともに、誤差データを更新し、
（Ｄ）通信部により、前記状態更新部による更新に関する更新データを、該演算通信装置が設けられた対象物体とは別の対象物体に送信し、
（Ｅ）該演算通信装置が設けられた対象物体とは別の対象物体から更新データを前記通信部により受けた場合に、状態統合部により、前記（Ｄ）で更新された最新の状態データと、前記通信部により受けた更新データとを統合することにより、新たな最新の状態データを求める、ことを特徴とする状態データ更新方法が提供される。
【発明の効果】
【００２４】
上述した本発明によると、複数の演算通信装置が、互いに別々の静止体または移動体にそれぞれ設けられ、各演算通信装置が、内部状態と外部状態の一方または両方を観測して観測値を取得し、この観測値に基づいて、複数の対象物体を含むシステムの状態を表わす状態データを更新する場合において、各演算通信装置は、更新に関する更新データを他の演算通信装置へ送信し、自身が更新した状態データと、他の演算通信装置から受けた更新データとを統合して新たな状態データを求める。
このように、各演算通信装置は、自身で更新した状態データと、他の演算通信装置から受けた更新データとを統合して新たな状態データを求める。この新たな状態データには、複数の対象物体の視点で得た観測値が反映されているので、統合された新たな状態データの精度が向上している。よって、複数の対象物体を含むシステムの状態を、より高精度に求めることができる。
【００２５】
統合された新たな状態データを求めるデータ処理を、各演算通信装置が行うので、データ処理が、１箇所の装置に集中することが回避される。すなわち、複数の対象物体を含むシステムの状態データを高精度に算出するための計算負荷を、複数の演算通信装置の間で分散できる。
【図面の簡単な説明】
【００２６】
【図１】本発明の対象となるシステムを示す。
【図２】複数の演算通信装置から構成される、本発明の実施形態による状態データ更新装置を示す。
【図３】カルマンフィルタを用いた場合の状態データ更新方法のフローチャートである。
【図４】情報フィルタを用いた場合の状態データ更新方法のフローチャートである。
【図５】観測値統合部を設けた場合の状態データ更新装置を示す。
【発明を実施するための形態】
【００２７】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２８】
図１は、本発明の対象となるシステム２０を示す。システム２０は、複数の対象物体３を含む。なお、図１は、鉛直方向に上方から複数の対象物体３を見た図である。また、図１の例では、３つの対象物体３が存在する場合を示しているが、本発明によると、システム２０を構成する対象物体３の数は、２つであっても、４つ以上であってもよい。
【００２９】
各対象物体３は、地上に静止している静止体であるか、または、地上に対して移動する移動体である。複数の対象物体３に、静止体と移動体の両方が含まれていてもよい。この例では、各対象物体３は、地上面に対して移動する移動ロボットである。各移動ロボット３は、車輪を有し、この車輪が地面に接触しながら回転駆動されることにより地上面を走行してよい。代わりに、各移動ロボット３は、歩行脚を有し、この歩行脚の動作により地上面に対して移動してもよい。
【００３０】
図２は、複数の演算通信装置５から構成される、本発明の実施形態による状態データ更新装置１０を示す。
【００３１】
状態データ更新装置１０は、複数の対象物体３を含むシステム２０の状態を求める装置群である。装置群は、複数の対象物体３にそれぞれ設けられる複数の演算通信装置５である。すなわち、状態データ更新装置１０は、装置群として、複数の演算通信装置５を備える。この装置群により、システム２０の状態を求める。
【００３２】
図２に基づいて、１つの対象物体３に設けられた演算通信装置５について説明するが、他の各対象物体３に設けられた演算通信装置５についても、以下で説明する構成と同じ構成を有し、以下で説明する処理と同じ処理を行う。なお、以下において、自身の対象物体３とは、以下で説明している演算通信装置５が設けられた対象物体３を意味し、別の対象物体３とは、以下で説明している演算通信装置５が設けられた対象物体３とは別の対象物体３を意味する。
【００３３】
演算通信装置５は、観測部７、状態推定部９、記憶部１１、状態更新部１３、通信部１５、および状態統合部１７を備える。
【００３４】
観測部７は、自身の対象物体３の内部状態と、自身の対象物体３に対する外部状態との一方または両方を観測する。これにより、観測部７は、自身の対象物体３の内部状態と自身の対象物体３に対する外部状態との一方または両方の観測値を取得する。観測部７は、このような観測値を、一定のサイクル時間毎に取得する。
観測部７が、後述の外界センサ７ｂとしてカメラを含む場合には、上述のサイクル時間は、一例では、１ｍｓ（ミリ秒）以上であって３００ｍｓ以下の値、別の例では、１０ｍｓ以上であって２００ｍｓ以下の値であり、さらに別の例では、５０ｍｓ以上であって２００ｍｓ以下の値（例えば、１００ｍｓ）である。
観測部７が、後述の外界センサ７ｂとしてレーザセンサを含む場合には、上述のサイクル時間は、一例では、１ｍｓ以上であって１００ｍｓ以下の値、別の例では、５ｍｓ以上であって１００ｍｓ以下の値であり、さらに別の例では、１０ｍｓ以上であって５０ｍｓ以下の値（例えば、２０ｍｓ）である。
【００３５】
内部状態は、自身の対象物体３に直接関係する状態を示す値である。本実施形態では、内部状態は、地上に対する自身の対象物体３（例えば移動ロボット）の位置、速度、加速度、および姿勢を含む。ただし、内部状態は、これらの位置、速度、加速度、および姿勢のいずれか（例えば、位置）であってもよいし、これらの位置、速度、加速度、および姿勢から選択された任意の組み合わせ（例えば、位置と速度と姿勢）であってもよい。また、本発明によると、内部状態は、他の値であってもよい。
【００３６】
演算通信装置５が、自身の対象物体３の観測部７により外部状態（例えば、別の対象物体３の状態）を観測する場合に、この外部状態は、別の対象物体３の外部からの観測により計測される、当該別の対象物体３の状態を示す値になる。この外部状態は、例えば、この観測部７が設けられた自身の対象物体３または地上に対する別の対象物体３（例えば移動ロボット）の位置と速度の一方（例えば位置）または両方を含む。ただし、本発明によると、演算通信装置５が自身の対象物体３の観測部７により観測する外部状態は、自身の対象物体３の外部の状態を示す他の値であってもよい。
【００３７】
観測部７は、内部状態を観測する内界センサ７ａと、外部状態を観測する外界センサ７ｂとを含む。本実施形態では、内界センサ７ａは、地上に対する自身の対象物体３（移動ロボット）の位置、速度、加速度、および姿勢をそれぞれ観測値として計測する位置計測部、加速度計測部、および姿勢計測部を含む。本実施形態では、外界センサ７ｂは、他の移動ロボット３の位置と速度を観測値として計測する。一例では、外界センサ７ｂは、周囲の形状を計測するレーザセンサ（例えば、ＬＲＦ）と、レーザセンサにより得た計測データを処理して別の対象物体３の位置と速度の一方（例えば位置）または両方を求めるデータ処理部とを含む。別の例では、外界センサ７ｂは、周囲の画像を取得するカメラと、カメラにより得た画像データを処理して別の対象物体３の位置と速度の一方（例えば位置）または両方を求めるデータ処理部とを含む。
【００３８】
観測部７は、時間間隔をおいて繰り返し観測を行い、観測を行う度に、上述した内部状態の観測値と上述した外部状態の観測値を取得する。
【００３９】
状態推定部９は、観測部７が観測を行う度に、当該観測時点におけるシステム２０の状態を表わす状態データと、状態データの誤差を表わす誤差データとを推定する。この推定は、推定式に従って行われる。推定式は、過去の観測時点におけるシステム２０の状態データを新たな観測時点の状態データへ遷移させるための計算式である。推定式については、後で詳しく述べる。観測部７が観測を行う度に、状態推定部９は、推定式と、記憶部１１に記憶された最新の状態データと最新の誤差データとに従って、当該観測時点における状態データとその誤差データを推定する。
【００４０】
記憶部１１は、状態推定部９により推定された状態データを記憶する。本実施形態では、記憶部１１は、推定された状態データとは別に、後述するように状態更新部１３により更新された最新の状態データを記憶する。初期時点においては、記憶部１１には、予め分かっている最新の状態データが記憶されていてよい。また、本実施形態では、記憶部１１は、状態データの誤差を表わす誤差データも記憶している。初期時点においては、記憶部１１には、予め分かっている最新の状態データの誤差データが最新の誤差データとして記憶されていてよい。
【００４１】
状態更新部１３は、観測部７が観測を行う度に、状態推定部９により推定された状態データと観測部７が得た観測値とに基づいて、システム２０の状態データを更新するとともに、この状態データの誤差データを更新する。これにより、更新された状態データと誤差データは、最新の状態データと誤差データとして記憶部１１に記憶される。すなわち、記憶部１１において、これらの最新の状態データと誤差データが、新たな最新の状態データと誤差データとなる。この時、以前に記憶された最新の状態データと誤差データは、記憶部１１から削除されてよい。
【００４２】
通信部１５は、状態更新部１３による更新に関する更新データ（後で詳しく述べる）を、別の対象物体３の演算通信装置５が有する通信部１５に送信し、別の対象物体３の演算通信装置５が有する通信部１５から更新データを受ける。
【００４３】
状態統合部１７は、自身の対象物体３における状態更新部１３により更新された最新の状態データと、別の対象物体３の通信部１５からの更新データとを統合することにより、新たな最新の状態データを求める。この新たな最新の状態データは、記憶部１１に記憶される。すなわち、記憶部１１において、この最新の状態データが、新たな最新の状態データとなる。この時、以前に記憶された最新の状態データは、記憶部１１から削除されてよい。状態統合部１７による統合方法については、後で詳しく説明する。
【００４４】
本実施形態によると、次のように、更新データを転送することもできる。状態データ更新装置１０を構成する複数の演算通信装置５は、この例では、少なくとも、第１、第２および第３の演算通信装置５を含む。第１の演算通信装置５の通信部１５と第２の演算通信装置５の通信部１５とが、障害物や他の要因により、互いに直接通信できない場合に、第１の演算通信装置５の通信部１５は、第１の演算通信装置５の状態更新部１３による更新に関する更新データを、第３の演算通信装置５の通信部１５を介して、第２の演算通信装置５の通信部１５へ送信可能である、この場合、第２の演算通信装置５の状態統合部１７は、第１の演算通信装置５からの更新データと、自身の対象物体３における状態更新部１３により更新された最新の状態データとを統合することにより、新たな最新の状態データを求める。
【００４５】
また、本実施形態によると、次のように、更新データを送信することもできる。状態データ更新装置１０を構成する複数の演算通信装置５は、この例では、少なくとも、第１、第２および第３の演算通信装置５を含む。第１の演算通信装置５の観測部７（外界センサ７ｂ）が、第２の演算通信装置５が設けられた対象物体３を観測して、この対象物体３の観測値を得た場合を想定する。この場合、この観測値（および観測部７が得た他の観測値）に基づいて、第１の演算通信装置５の状態更新部１３が上述のように状態データと誤差データを更新する。次いで、この更新に関する更新データを、第１の演算通信装置５の通信部１５が、第１および第２の演算通信装置５とは別の第３の演算通信装置５の通信部１５へ送信する。この場合、第３の演算通信装置５の状態統合部１７は、第１の演算通信装置５からの更新データと、自身の対象物体３における状態更新部１３により更新された最新の状態データとを統合することにより、新たな最新の状態データを求める。
【００４６】
上述した演算通信装置５は、カルマンフィルタを用いるように構成されてもよいし、情報フィルタを用いるように構成されてもよい。上述の各内容は、カルマンフィルタを用いる場合にも、情報フィルタを用いる場合にも適用される。
【００４７】
（カルマンフィルタを用いる演算通信装置）
演算通信装置５がカルマンフィルタを用いる場合について説明する。
【００４８】
この場合、上述の状態データは、システム２０の状態を直接的に表す状態ベクトルである。状態ベクトルは、システム２０を構成する複数の対象物体３のそれぞれの状態を示す複数の要素からなる。状態ベクトルｕは、次の式（１）で表される。
【００４９】
【数１】
0006541026.tif 000002
【００５０】
式（１）において、記号Ｔは、ｕが列ベクトルであることを示し（以下、ベクトルに付された他のＴも同様）、状態ベクトルｕの添え字１，２，・・・Ｎは、対象物体３を互いに識別するための連続する識別番号（正の整数）である。Ｎは、システム２０に存在する対象物体の数に等しい（以下、同様）。
【００５１】
また、本実施形態では、各対象物体３の状態ベクトルｕｉ（ｉは、１〜Ｎまでの整数）は、次の式（２）で表わされる。なお、以下において、各記号や符号の添え字ｉは、対象物体３を識別するための上述の識別番号を意味する。例えば、対象物体３ｉは、ｉにより識別される対象物体を示す。
【００５２】
【数２】
0006541026.tif 000003
【００５３】
式（２）において、各記号は、次の通りである。
ｘｉ，ｙｉ，ｚｉは、それぞれ、対象物体３ｉの位置を示す。すなわち、ｘｉ，ｙｉ，ｚｉは、それぞれ、対象物体３ｉのｘ座標、ｙ座標、ｚ座標である。なお、ｘ座標、ｙ座標、ｚ座標は、それぞれ、地上に固定された静止座標系の互いに直交するｘ座標軸、ｙ座標軸、ｚ座標軸における座標である。
θｉ，φｉ，αｉは、それぞれ、上述の静止座標系に対する対象物体３ｉの姿勢を表わす角度である。
ｘｉ，ｙｉ，ｚｉ，θｉ，φｉ，αｉの上部に付された１つの黒丸（ドット・）は、時間の１回微分（すなわち、それぞれ速度と角速度）を意味し、ｘｉ，ｙｉ，ｚｉの上部に付された２つの黒丸（ドットドット・・）は、時間の２回微分（すなわち、加速度）を意味する。
【００５４】
また、上述の誤差データは、上述の状態ベクトルｕの誤差共分散行列Ｐである。この誤差共分散行列Ｐは、次の式（３）で表わされる。
【００５５】
【数３】
0006541026.tif 000004
【００５６】
式（３）において、Ｐｉ（ｉは、１〜Ｎまでの整数）は、対象物体３ｉの状態ベクトルｕｉの誤差共分散行列である。また、Ｐｉ，ｊは、対象物体３ｉと対象物体３ｊ（ｊは、１〜Ｎまでの整数のうちｉを除くもの）の間における部分共分散行列である。
【００５７】
以下、カルマンフィルタを用いる場合において、１つの対象物体３に設けられた演算通信装置５について説明するが、他の各対象物体３に設けられた演算通信装置５についても、以下で説明する構成と同じ構成を有し、以下で説明する処理と同じ処理を行う。
【００５８】
状態推定部９は、観測部７が観測を行う度に、当該観測時点ｋにおけるシステム２０の状態を表わす状態ベクトルｕと、当該観測時点ｋにおける状態ベクトルｕの誤差共分散行列Ｐを推定する。状態ベクトルｕの推定は、次の式（４）で表わされる推定式に従って行われる。誤差共分散行列Ｐの推定は、次の式（５）で表わされる推定式に従って行われる。
ｕｋ’＝ｆ（ｕｋ−１，βｋ）    ・・・（４）
Ｐｋ’＝ＦｋＰｋＦｋＴ＋Ｑｋ    ・・・（５）
【００５９】
この式（４）において、各記号の添え字ｋ、（ｋ−１）は、自然数であり時点を示す（以下、同様）。
ｕｋ−１は、記憶部１１に記憶されている最新の状態ベクトルｕであり、ｕの添え字（ｋ−１）は、過去の時点（ｋ−１）において取得された観測値に基づいて、更新された状態ベクトルｕであることを示す。
また、βｋは、時点ｋで得られた入力値であり、上述の内部状態である。例えば、βｋは、対象物体３の速度と加速度であり、この対象物体３の加速度計測部と速度計測部により計測される。ただし、βｋは、他の値であってもよい。
【００６０】
式（５）において、Ｑｋは、プロセスノイズの誤差共分散行列であり、Ｆｋは、次の式（６）で表わされる係数行列である。
【００６１】
【数４】
0006541026.tif 000005
【００６２】
さらに、状態推定部９は、上述のように推定した状態ベクトルｕｋ’と、観測部７が取得した観測値ｚとのマッチングを行う。例えば、観測部７が取得した観測値ｚに、別の対象物体３ｉの位置ｘｉ，ｙｉ，ｚｉが含まれている場合に、これらの位置ｘｉ，ｙｉ，ｚｉが、状態ベクトルｕｋ’におけるどの値に対応するのかを特定する。このようなマッチングは、カイ二乗検定や他の方法により行うことができる。
【００６３】
その後、状態更新部１３は、状態推定部９により推定された状態ベクトルｕｋ’および誤差共分散行列Ｐｋ’と、観測値（上述のマッチングによりｕｋ’に含まれる値に対応づけられた観測値を含む）とに基づいて、状態ベクトルｕと誤差共分散行列Ｐを更新する。この更新は、次の式（７）（８）により行われる。
ｕｋ＝ｕｋ’＋Ｋｋ（ｚｋ−Ｈｋｕｋ’）    ・・・（７）
Ｐｋ＝（Ｉ−ＫｋＨｋ）Ｐｋ’    ・・・（８）
【００６４】
この式（７）において、ｕｋは、更新された、時点ｋ（観測時点ｋ）の状態ベクトルｕである。式（８）において、Ｐｋは、更新された、時点ｋの誤差共分散行列Ｐである。
また、式（７）（８）において、Ｈｋは、次の式（９）で表わされる係数行列である。
【００６５】
【数５】
0006541026.tif 000006
【００６６】
式（９）において、ｈは、次の式（１０）で表わされる観測方程式である。
ｚｋ＝ｈ（ｕｋ）    ・・・（１０）
この式（１０）において、ｈは、システム２０の状態ベクトルｕｋの関数であり、ｚｋは、観測時点ｋにおいて観測部７が得た観測値であり、自身の対象物体３に設けた観測部７が観測した上述の外部状態である。
【００６７】
式（７）（８）において、Ｋｋは、カルマンゲインであり、次の式（１１）で表わされる。
Ｋｋ＝Ｐｋ’ＨｋＴＳｋ−１    ・・・（１１）
【００６８】
また、式（１１）において、Ｓｋは、次の式（１２）で表わされる。
Ｓｋ＝ＨｋＰｋ’−１ＨｋＴ    ・・・（１２）
【００６９】
上述のように更新された観測時点ｋの状態ベクトルｕｋと誤差共分散行列Ｐｋは、それぞれ、新たな最新の状態ベクトルｕｋと誤差共分散行列Ｐｋとして記憶部１１に記憶される。
【００７０】
また、演算通信装置５は、自身の状態更新部１３により更新した観測時点ｋの状態ベクトルｕｋを、または、この状態ベクトルｕｋおよびその誤差共分散行列Ｐｋを、上述の更新データとして、自身の通信部１５により、別の対象物体３の通信部１５へ送信する。
【００７１】
一方、演算通信装置５が、別の対象物体３の通信部１５から、更新された状態ベクトルｕを、自身の通信部１５により受信した場合には、次のように、この状態ベクトルｕと、自身の記憶部１１に記憶されている最新の状態ベクトルｕとを統合する。
【００７２】
これについて、本実施形態によると、演算通信装置５ｉの状態統合部１７ｉ（ここで、ｉは、演算通信装置５が設けられた対象物体３の識別番号である。以下、同様）は、この演算通信装置５ｉが求めた最新の状態ベクトルｕ（すなわち、記憶部１１ｉに記憶されている最新の状態ベクトルｕ）と、別の対象物体３ｊ（ここで、ｉ≠ｊ）の通信部１５ｊから受信した状態ベクトルｕとにそれぞれ、予め定めた重み係数を乗じたベクトル同士を足し合わせることにより、統合した新たな最新の状態ベクトルｕを求める。すなわち、状態統合部１７ｉは、次の式（１３ａ）に従って、統合した新たな最新の状態ベクトルｕＣを求める。
ｕＣ  ＝  ｗｉ×ｕＳ＋ｗｊ×ｕＤ    ・・・（１３ａ）
この式（１３ａ）において、ｕＣは、統合された新たな最新の状態ベクトルｕであり、ｕＳは、演算通信装置５ｉにより求められ記憶部１１ｉに記憶されている最新の状態ベクトルｕであり、ｕＤは、演算通信装置５ｉが別の対象物体３ｊの通信部１５から受けた状態ベクトルｕである。また、ｗｉとｗｊは、重み係数であり、ｗｉ＋ｗｊ＝１である。ｗｉとｗｊは、予め設定されていてよく、例えば、それぞれ、１／２である。または、ｗｉは、ｗｊよりも大きく、かつ、１よりも小さくてもよい。
【００７３】
上述のように統合された最新の状態ベクトルｕＣは、記憶部１１ｉに、新たな最新の状態ベクトルｕとして記憶される。
【００７４】
演算通信装置５の状態統合部１７は、状態ベクトルの統合に加えて、誤差共分散行列の統合をさらに行ってもよい。すなわち、演算通信装置５ｉの状態統合部１７ｉは、この演算通信装置５ｉが求めた最新の誤差共分散行列Ｐ（すなわち、記憶部１１ｉに記憶されている最新の誤差共分散行列Ｐ）と、別の対象物体３ｊ（ここで、ｉ≠ｊ）の通信部１５ｊから受信した誤差共分散行列Ｐとにそれぞれ、予め定めた重み係数を乗じた誤差共分散行列同士を足し合わせることにより、統合した新たな最新の誤差共分散行列Ｐを求める。すなわち、状態統合部１７ｉは、次の式（１３ｂ）に従って、統合した新たな最新の誤差共分散行列ＰＣを求める。
ＰＣ  ＝  Ｗｉ×ＰＳ＋Ｗｊ×ＰＤ    ・・・（１３ｂ）
この式（１３ｂ）において、ＰＣは、統合された新たな誤差共分散行列Ｐであり、ＰＳは、演算通信装置５ｉにより求められ記憶部１１ｉに記憶されている最新の誤差共分散行列Ｐであり、ＰＤは、演算通信装置５ｉが別の対象物体３ｊの通信部１５から受けた誤差共分散行列Ｐである。また、ＷｉとＷｊは、重み係数であり、Ｗｉ＋Ｗｊ＝１である。ＷｉとＷｊは、予め設定されていてよく、例えば、それぞれ、１／２である。または、Ｗｉは、Ｗｊよりも大きく、かつ、１よりも小さくてもよい。
【００７５】
上述のように統合された最新の誤差共分散行列ＰＣは、記憶部１１ｉに、新たな最新の誤差共分散行列Ｐとして記憶される。
【００７６】
図３は、本発明の実施形態による状態データ更新方法のフローチャートである。図３は、カルマンフィルタを用いた場合を示す。この状態データ更新方法は、上述した演算通信装置５により行われる。
【００７７】
ステップＳ１において、観測部７により上述の観測値を取得する。
【００７８】
ステップＳ２において、状態推定部９により、ステップＳ１で観測値を取得した観測時点ｋにおけるシステム２０の状態を表わす状態ベクトルｕと、当該観測時点ｋにおける状態ベクトルｕの誤差共分散行列Ｐを推定する。これにより、推定された状態ベクトルｕｋ’と、推定された誤差共分散行列Ｐｋ’を得る。
【００７９】
ステップＳ３において、ステップＳ２で得た状態ベクトルｕｋ’と、ステップＳ１で得た観測値ｚｋとの上記マッチングを行う。これにより、観測値ｚｋが、状態ベクトルｕｋ’におけるどの値に対応するのかを特定する。
【００８０】
ステップＳ４において、状態更新部１３により、ステップＳ２で推定された状態ベクトルｕｋ’および誤差共分散行列Ｐｋ’と、ステップＳ１で得た観測値ｚｋとに基づいて、上述のように、状態ベクトルｕと誤差共分散行列Ｐを更新する。更新された状態ベクトルｕと誤差共分散行列Ｐは、最新の状態ベクトルｕと誤差共分散行列Ｐとして記憶部１１に記憶される。
【００８１】
ステップＳ５において、演算通信装置５は、自身の状態更新部１３により更新した観測時点ｋの状態ベクトルｕｋを、自身の通信部１５により、別の対象物体３の通信部１５へ送信する。
【００８２】
ステップＳ６において、演算通信装置５は、別の対象物体３の通信部１５から、更新された状態ベクトルｕｋを、自身の対象物体３の通信部１５により、現時点までに受信したかどうかを判断する。この判断の結果が肯定である場合には、ステップＳ７へ進み、そうでない場合には、一定時間後に再びステップＳ６を行うか、または、ステップＳ１へ戻る。
【００８３】
ステップＳ７において、状態統合部１７により、受信された状態ベクトルｕと、自身の記憶部１１に記憶されている最新の状態ベクトルｕとを上述のように統合する。統合された状態ベクトルｕＣは、記憶部１１に、新たな最新の状態ベクトルｕとして記憶される。ステップＳ７の後、ステップＳ１へ戻り、上述の処理を繰り返す。
【００８４】
なお、上述の処理を繰り返している時に、状態データ更新装置１０の動作を停止させる場合には、図３のフローを終了する。
【００８５】
（情報フィルタを用いる演算通信装置）
演算通信装置５が情報フィルタを用いる場合について説明する。なお、以下で述べない点は、上述のカルマンフィルタを用いた場合と同じである。特に、上述と同じ各記号は、上述と同じものを示す。
【００８６】
この場合、上述の状態データは、上述の式（１）で表わされる状態ベクトルｕから変換された情報ベクトルζである。また、誤差データは、状態ベクトルｕの誤差共分散行列Ｐから変換された情報行列Λである。
【００８７】
具体的には、情報行列Λと情報ベクトルζは、それぞれ、次の式（１４）（１５）で表わされる。
Λ  ＝  Ｐ−１    ・・・（１４）
ζ  ＝  Λｕ    ・・・（１５）
【００８８】
以下、情報フィルタを用いる場合において、１つの対象物体３に設けられた演算通信装置５について説明するが、他の各対象物体３に設けられた演算通信装置５についても、以下で説明する構成と同じ構成を有し、以下で説明する処理と同じ処理を行う。
【００８９】
状態推定部９は、推定式に従って、観測部７が観測値を取得した観測時点ｋにおける情報行列Λｋ’と情報ベクトルζｋ’を推定する。推定式は、次の式（１６）（１７）で表わされる。
Λｋ’＝（ＦｋΛｋ−１−１ＦｋＴ＋Ｑｋ）−１    ・・・（１６）
ζｋ’＝Λｋ’ｕｋ’    ・・・（１７）
【００９０】
この式（１６）において、ｕｋ’は、上述した式（４）により推定された状態ベクトルｕである。また、Λｋ−１は、記憶部１１に記憶されている最新の情報行列である。Λｋ’とζｋ’は、それぞれ、推定された観測時点ｋの情報行列と情報ベクトルである。他の記号は、カルマンフィルタの場合で説明した同じ記号と同じものを示す。
【００９１】
さらに、状態推定部９は、上述のように推定した情報ベクトルζｋ’（状態ベクトルｕｋ’に対応）と、観測部７が取得した観測値とのマッチングを行う。例えば、観測部７が取得した観測値に、別の対象物体３ｉの位置ｘｉ，ｙｉ，ｚｉが含まれている場合に、これらの位置ｘｉ，ｙｉ，ｚｉが、情報ベクトルζｋ’（状態ベクトルｕｋ’に対応）におけるどの値に対応するのかを特定する。このようなマッチングは、カイ二乗検定や他の方法により行うことができる。
【００９２】
例えば、カイ二乗検定により、マッチングを以下のように行うことができる。
【００９３】
状態推定部９は、外界センサ７ｂの観測により得た、別の対象物体３の観測値ｚｉ（例えば、対象物体３ｉの位置や速度）が、既知の別の対象物体３ｊに関する値に対応するかどうか（ｉ＝ｊであるか）を判断する。なお、既知の別の対象物体３ｊに関する値は、記憶部１１に記憶されている最新の情報ベクトルζに対応する最新の状態ベクトルｕに含まれている値（ｕｊ）である。最新の状態ベクトルｕも、記憶部１１に記憶されている。
【００９４】
状態推定部９は、観測値ｚｉが既知の別の対象物体３ｊに関する値に対応するかどうか（ｉ＝ｊであるか）を判断するために、次の不等式（１８）が満たされるかどうかを判断する。状態推定部９は、不等式（１８）が満たされると判断した場合には、観測値ｚｉが既知の別の対象物体３ｊに関する値に対応すると判断する。すなわち、状態推定部９は、観測値ｚｉが既知の別の対象物体３ｊの値であると判断する。
Ａ２≧μｊｉＴＳｋ−１μｊｉ    ・・・（１８）
【００９５】
この式（１８）において、Ａは、予め設定された値（カイ値）である。μｊｉは、次の式（１９）で表わされる。
μｊｉ＝ｐｊ−ｚｉ    ・・・（１９）
ここで、ｐｊは、既知の別の対象物体３ｊの位置を示すベクトル（ｘｊ，ｙｊ，ｚｊ）Ｔであり、ｚｉは、今回の観測時点ｋで得られた観測値ｚｉ（対象物体３ｉの位置）を示すベクトルである。
【００９６】
マッチングの後、状態更新部１３は、状態推定部９により推定された情報行列Λｋ’および情報ベクトルζｋ’と、観測値（上述のマッチングによりｕｋ’に含まれる値に対応づけられた観測値を含む）とに基づいて、情報行列Λと情報ベクトルζを更新する。この更新は、次の式（２０）（２１）により行われる。
Λｋ＝Λｋ’＋ｇΛｋ，ｄ    ・・・（２０）
ζｋ＝ζｋ’＋ｇζｋ，ｄ    ・・・（２１）
【００９７】
この式（２０）において、Λｋ，ｄは、更新された情報行列Λｋと推定された情報行列Λｋ’との差に関する情報行列差分量である。式（２１）において、ζｋ，ｄは、更新された情報ベクトルζｋと推定された情報ベクトルζｋ’との差に関する情報ベクトル差分量である。Λｋ，ｄとζｋ，ｄは、それぞれ、次の式（２２）（２３）で表わされる。
Λｋ，ｄ＝ＨｋＴＲ−１Ｈｋ    ・・・（２２）
ζｋ，ｄ＝ＨｋＴＲ−１（ｚｋ−ｈ（ｕｋ’）＋Ｈｋｕｋ’）・・・（２３）
式（２２）（２３）において、Ｒは、観測部７に関する観測ノイズの誤差共分散行列である。
【００９８】
状態更新部１３は、情報行列Λと情報ベクトルζを更新する時に、上述の情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄも求める。
【００９９】
上述のように更新された観測時点ｋの情報行列Λｋと情報ベクトルζｋは、それぞれ、新たな最新の情報行列Λと情報ベクトルζとして記憶部１１に記憶される。
【０１００】
また、演算通信装置５は、自身の状態更新部１３により求めた情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄを、上述の更新データとして、自身の通信部１５により、別の対象物体３の通信部１５へ送信する。
【０１０１】
一方、演算通信装置５が、別の対象物体３の通信部１５から、情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄを、自身の通信部１５により受信した場合には、統合処理を行う。すなわち、演算通信装置５の状態統合部１７は、受信した情報行列差分量Λｋ，ｄと、自身の記憶部１１に記憶されている最新の情報行列Λとを足し合わせることにより、統合した最新の情報行列ΛＣを求めるとともに、受信した情報ベクトル差分量ζｋ，ｄと、自身の記憶部１１に記憶されている最新の情報ベクトルζとを足し合わせることにより、統合した最新の情報ベクトルζＣを求める。
【０１０２】
すなわち、状態統合部１７は、次の式（２４）（２５）に従って、統合した新たな最新の情報行列ΛＣと情報ベクトルζＣを求める。
ΛＣ＝ΛＳ＋Λｋ，ｄ    ・・・（２４）
ζＣ＝ζＳ＋ζｋ，ｄ    ・・・（２５）
この式（２４）において、ΛＣは、統合された新たな最新の情報行列であり、ΛＳは、演算通信装置５により求められ記憶部１１に記憶されている最新の情報行列である。式（２５）において、ζＣは、統合された新たな最新の情報ベクトルであり、ζＳは、演算通信装置５により求められ記憶部１１に記憶されている最新の情報ベクトルである。
【０１０３】
図４は、本発明の実施形態による状態データ更新方法のフローチャートである。図４は、情報フィルタを用いた場合を示す。この状態データ更新方法は、上述した演算通信装置５により行われる。
【０１０４】
ステップＳ１１において、観測部７により上述の観測値を取得する。
【０１０５】
ステップＳ１２において、状態推定部９により、ステップＳ１１で観測値を取得した観測時点ｋにおける情報行列Λと情報ベクトルζを推定する。これにより、推定された情報行列Λｋ’と、推定された情報ベクトルζｋ’を得る。
【０１０６】
ステップＳ１３において、ステップＳ１２で得た情報ベクトルζｋ’に対応する状態ベクトルｕｋ’と、ステップＳ１で得た観測値ｚｋとのマッチングを行う。これにより、観測値ｚｋが、状態ベクトルｕｋ’（すなわち、情報ベクトルζｋ’）におけるどの値に対応するのかを特定する。
【０１０７】
ステップＳ１４において、状態更新部１３により、ステップＳ３で推定された情報行列Λｋ’および情報ベクトルζｋ’と、ステップＳ１１で得た観測値ｚｋとに基づいて、上述のように、情報行列Λと情報ベクトルζを更新する。更新された情報行列Λと情報ベクトルζは、最新の情報行列Λと情報ベクトルζとして記憶部１１に記憶される。
【０１０８】
また、ステップＳ１４では、状態更新部１３により、上述の情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄも求める。
【０１０９】
ステップＳ１５において、演算通信装置５は、自身の状態更新部１３により求めた情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄを、自身の通信部１５により、別の対象物体３の通信部１５へ送信する。
【０１１０】
ステップＳ１６において、演算通信装置５は、別の対象物体３の通信部１５から、情報行列差分量Λｄと情報ベクトル差分量ζｄを、自身の対象物体３の通信部１５により、現時点までに受信したかどうかを判断する。この判断の結果が肯定である場合には、ステップＳ１７へ進み、そうでない場合には、一定時間後に再びステップＳ１６を行うか、または、ステップＳ１１へ戻る。
【０１１１】
ステップＳ１７において、状態統合部１７により、受信された情報行列差分量Λｄと、自身の記憶部１１に記憶されている最新の情報行列Λとを上述のように統合する。また、ステップＳ１７において、状態統合部１７により、受信された情報ベクトル差分量ζｄと、自身の記憶部１１に記憶されている最新の情報ベクトルζとを上述のように統合する。統合により得た情報行列ΛＣと情報ベクトルζＣは、新たな最新の情報行列Λと情報ベクトルζとして記憶される。ステップＳ１７の後、ステップＳ１１へ戻り、上述の処理を繰り返す。
【０１１２】
なお、上述の処理を繰り返している時に、状態データ更新装置１０の動作を停止させる場合には、図４のフローを終了する。
【０１１３】
上述の統合をまとめて行う場合を説明する。ここで、ステップＳ１４を行った時に、このステップＳ１４を今回の更新処理とし、今回に対する前回のステップＳ１４を前回の更新処理とする。今回のステップＳ１４を行った時に、複数回、演算通信装置５の通信部１５が他の演算通信装置５から情報行列差分量Λｄと情報ベクトル差分量ζｄを受けた場合には、今回のステップＳ１４の直後のステップＳ１７において、状態統合部１７は、次のようにまとめて統合を行う。
【０１１４】
状態統合部１７は、前回の更新処理（ステップＳ１４）が行われた時点から今回の更新処理（ステップＳ１４）が行われた時点までの期間Ｄにおいて、演算通信装置５の通信部１５が他の演算通信装置５から受けたすべての情報行列差分量Λｄと今回の更新処理で求めた新たな最新の情報行列Λとを、次式（２６）に従って足し合わせることにより、統合された新たな最新の情報行列ΛＣを求める。また、状態統合部１７は、この期間Ｄにおいて、演算通信装置５の通信部１５が他の演算通信装置５から受けたすべての情報ベクトル差分量ζｄと今回の更新処理で求めた新たな最新の情報ベクトルζとを、次式（２７）に従って足し合わせることにより、統合された新たな最新の情報ベクトルζＣを求める。
【０１１５】
ΛＣ＝Λ＋Λｄ１＋Λｄ２＋・・・＋Λｄｍ    ・・・（２６）
ζＣ＝ζ＋ζｄ１＋ζｄ２＋・・・＋ζｄｍ    ・・・（２７）
【０１１６】
式（２６）において、Λｄ１，Λｄ２，・・・Λｄｍは、それぞれ、前回のステップＳ１４の後、１回目に受信した情報行列差分量Λｄ、２回目に受信した情報行列差分量Λｄ、・・・ｍ回目（ｍは、２以上の整数である）に受信した情報行列差分量Λｄである。ただし、ｍ＝２の場合には、式（２６）は、ΛＣ＝Λ＋Λｄ１＋Λｄ２となる。
同様に、式（２７）において、ζｄ１，ζｄ２，・・・ζｄｍは、それぞれ、前回のステップＳ１４の後、１回目に受信した情報ベクトル差分量ζｄ、２回目に受信した情報ベクトル差分量ζｄ、・・・ｍ回目（ｍは、２以上の整数である）に受信した情報ベクトル差分量ζｄである。ただし、ｍ＝２の場合には、式（２７）は、ζＣ＝ζ＋ζｄ１＋ζｄ２となる。
【０１１７】
上述した本発明の実施形態によると、複数の演算通信装置５が、互いに別々の静止体または移動体にそれぞれ設けられ、各演算通信装置５が、内部状態と外部状態の一方または両方を観測して観測値を取得し、この観測値に基づいて、複数の対象物体３を含むシステム２０の状態を表わす状態データを更新する場合において、各演算通信装置５は、更新に関する更新データを他の演算通信装置５へ送信し、自身が更新した状態データと、他の演算通信装置５から受けた更新データとを統合して新たな状態データを求める。
このように、各演算通信装置５は、自身で更新した状態データと、他の演算通信装置５から受けた更新データとを統合して新たな状態データを求める。この新たな状態データには、複数の対象物体３の視点で得た観測値が反映されているので、統合された新たな状態データの精度が向上している。よって、複数の対象物体３を含むシステム２０の状態を、より高精度に求めることができる。
【０１１８】
統合された新たな状態データを求めるデータ処理を、各演算通信装置５が行うので、データ処理が、１箇所の装置に集中することが回避される。すなわち、複数の対象物体３を含むシステム２０の状態データを高精度に算出するための計算負荷を、複数の演算通信装置５の間で分散できる。したがって、各演算通信装置５において、最新の状態データ（および誤差データ）を短時間で算出できる。
【０１１９】
本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。例えば、以下の他の実施形態１〜３のいずれかを採用してもよいし、他の実施形態１〜３の複数を任意に組み合わせて採用してもよい。この場合、以下で説明しない点は上述と同じである。
【０１２０】
（他の実施形態１）
上述では、各演算通信装置５は、自身の状態更新部１３がシステム２０の状態データを更新する度に、この更新データを、この演算通信装置５が設けられた対象物体３とは別の対象物体３へ送信していた。しかし、本発明によると、各演算通信装置５は、自身の状態更新部１３による更新に関する更新データを送信する送信タイミングを指定してもよい。すなわち、各演算通信装置５は、自身の状態更新部１３により更新した更新データを、この演算通信装置５が設けられた対象物体３とは別の対象物体３へ送信する送信タイミングを指定してもよい。この場合、各演算通信装置５は、指定した送信タイミングで、自身の状態更新部１３により更新した更新データ（最新の更新データ）を、この演算通信装置５が設けられた対象物体３とは別の対象物体３へ送信する。
【０１２１】
例えば、各演算通信装置５は、この演算通信装置５が設けられた対象物体３の観測部７が観測した別の対象物体３の位置または速度（外部状態）に基づいて、この別の対象物体３が静止しているかどうかを判断する。ここで、この演算通信装置５は、別の対象物体３が静止していると判断したら、自身の状態更新部１３がシステム２０の状態データを更新するサイクルの時間（すなわち、観測部７が観測を行う上述のサイクル時間と同じ時間）よりも長い時間を送信サイクル（送信タイミング）として指定する。このように、別の対象物体３が静止している場合には、別の対象物体３の位置または速度を高精度に計測する必要性が低下するので、該別の対象物体３の位置または速度の観測値を、自身の対象物体３から該別の対象物体３または他の別の対象物体３へ送信する頻度を下げる。
【０１２２】
（他の実施形態２）
各演算通信装置５は、自身の状態更新部１３による更新に関する更新データの送信先とする別の対象物体３を選択してもよい。例えば、各演算通信装置５は、この演算通信装置５が設けられた自身の対象物体３から設定距離以内に位置する別の対象物体３を送信先として選択してもよい。この場合、次いで、この演算通信装置５は、送信先として選択した１つ又は複数の別の対象物体３に更新データを送信する。
【０１２３】
各演算通信装置５がカルマンフィルタを用いる場合には、上述のステップＳ５において、演算通信装置５は、この演算通信装置５が設けられた自身の対象物体３から設定距離以内に位置する別の対象物体３を送信先として選択する。次いで、上述のステップＳ５において、この演算通信装置５は、自身の状態更新部１３により更新した観測時点ｋの状態ベクトルｕｋ（すなわち、更新データ）を、送信先として選択した別の対象物体３の通信部１５へ自身の通信部１５により送信する。
【０１２４】
なお、演算通信装置５は、自身の対象物体３の記憶部１１に記憶された最新の状態データ（別の対象物体２の位置を含むデータ）に基づいて、自身の対象物体３から設定距離以内に位置する別の対象物体３を送信先として選択してよい。この点は、後述のように情報フィルタを用いる場合も同様である。
【０１２５】
各演算通信装置５が情報フィルタを用いる場合には、上述のステップＳ１５において、演算通信装置５は、この演算通信装置５が設けられた自身の対象物体３から設定距離以内に位置する別の対象物を送信先として選択する。次いで、上述のステップＳ１５において、この演算通信装置５は、自身の状態更新部１３により求めた情報行列差分量Λｋ，ｄと情報ベクトル差分量ζｋ，ｄ（すなわち、更新データ）を、送信先として選択した別の対象物体３の通信部１５へ自身の通信部１５により送信する。
【０１２６】
このように、各演算通信装置５は、この演算通信装置５が設けられた対象物体３から設定距離以内に位置する別の対象物体３を送信先として選択する。一方、各演算通信装置５は、この演算通信装置５が設けられた対象物体３から設定距離より離れて位置する別の対象物体３を送信先として選択しないようにすることができる。これにより、各演算通信装置５は、この演算通信装置５が設けられた対象物体３から直接観測できる別の対象物体３を送信先とし、この演算通信装置５が設けられた対象物体３から直接観測できない遠方の別の対象物体３へは更新データを送信先としない。これにより、更新データの送信先を減らして、更新データを、システム２０の最新の状態データに効率よく反映させることができる。
【０１２７】
（他の実施形態３）
例えば、図５に示すように、各演算通信装置５が、さらに観測値統合部１９を有していてもよい。
【０１２８】
以下において、１つの対象物体３に設けられた演算通信装置５について説明するが、他の各対象物体３に設けられた演算通信装置５についても、以下で説明する構成と同じ構成を有し、以下で説明する処理と同じ処理を行う。なお、以下において、自身の対象物体３とは、以下で説明している演算通信装置５が設けられた対象物体３を意味し、別の対象物体３とは、以下で説明している演算通信装置５が設けられた対象物体３とは別の対象物体３を意味する。
【０１２９】
通信部１５は、観測部７が取得した外部状態（ここでは、自身の対象物体３または地上に対する別の対象物体３の位置と速度の一方または両方）の観測値を取得した時に、この観測値を、この別の対象物体３の演算通信装置５に設けられた通信部１５に送信する。
【０１３０】
さらに、自身の対象物体３の位置と速度の一方または両方の観測値が、別の対象物体３における観測部７により外部状態の観測値として取得された場合には、自身の対象物体３の通信部１５は、この別の対象物体３における通信部１５から、この外部状態の観測値を受け、この観測値が記憶部１１に記憶される。その後、自身の対象物体３における観測部７が、内部状態（自身の対象物体３の位置と速度の一方または両方）の観測値を取得した時に、観測値統合部１９は、この観測値と、別の対象物体３における通信部１５から送信され記憶部１１に記憶されている観測値（自身の対象物体３の位置と速度の一方または両方）とを統合する。この統合は、例えば、次のように行われてよい。
【０１３１】
演算通信装置５ｉの観測値統合部１９ｉ（ここで、ｉは、演算通信装置５が設けられた対象物体３の識別番号である。以下、同様）は、この演算通信装置５ｉの観測部７ｉが取得した観測値ｚと、別の対象物体３ｊ（ここで、ｉ≠ｊ）の通信部１５ｊから受信し記憶部１１ｉに記憶されている観測値ｚとに，それぞれ、予め定めた重み係数を乗じた値同士を足し合わせることにより、統合した新たな観測値ｚを求める。すなわち、観測値統合部１９ｉは、次の式（２８）に従って、統合した観測値ｚＣを求める。
ｚＣ  ＝  Ｍｉ×ｚＳ＋Ｍｊ×ｚＤ    ・・・（２８）
この式（２８）において、ｚＣは、統合された観測値ｚであり、ｚＳは、観測部７ｉが取得した観測値ｚであり、ｚＤは、別の対象物体３ｊ（ここで、ｉ≠ｊ）の観測部７ｊにより取得され通信部１５ｊから送信されて記憶部１１ｉに記憶されている観測値ｚである。また、ＭｉとＭｊは、重み係数であり、Ｍｉ＋Ｍｊ＝１である。ＭｉとＭｊは、予め設定されていてよく、例えば、それぞれ、１／２である。または、Ｍｉは、Ｍｊよりも大きく、かつ、１よりも小さくてもよい。
【０１３２】
なお、上述の統合は、対象物体３ｉの位置同士と速度同士の一方または両方について行われる。また、この統合のために、ｚＳとｚＤは、同じ座標系の座標値で表わされている。例えば、ｚＳは、地上に固定された静止座標系の座標値で表わされ、ｚＤも、この静止座標系の座標値で表わされた状態で、通信部１５ｊから通信部１５ｉへ送信される。
【０１３３】
このように観測値が統合された場合、状態更新部１３は、状態推定部９により推定された状態データと統合された観測値とに基づいて、システム２０の状態データを更新するとともに、この状態データの誤差データを更新する。すなわち、上述において、「観測部７が得た観測値」や他の同様の記載は、「統合された観測値」に読み替えられる。
【符号の説明】
【０１３４】
３  対象物体（移動ロボット）、５  演算通信装置、７  観測部、７ａ  内界センサ、７ｂ  外界センサ、９  状態推定部、１０  状態データ更新装置、１１  記憶部、１３  状態更新部、１５  通信部、１７  状態統合部、１９  観測値統合部、２０  システム
【図１】
0006541026.tif 000007
【図２】
0006541026.tif 000008
【図３】
0006541026.tif 000009
【図４】
0006541026.tif 000010
【図５】
0006541026.tif 000011