ロボットの運動予測制御方法と装置
【課題】状態推定の計算量やデータ通信の所要時間の影響を受けることなく、ロボットごとに決められた制御周期で制御指令値を算出し、複数のロボットを制御することができるロボットの運動予測制御方法と装置を提供する。
【解決手段】（Ａ）ロボット１の制御周期に依らない任意のタイミングで、計測装置１２、状態推定装置１４、及びデータ記憶装置１６により、対象物やロボット１のセンサ情報を取得し、センサ情報と同時刻の各ロボット１の内部状態Ｘを予測し、センサ情報と比較して内部状態を更新し、更新した内部状態Ｘと予測に用いた状態遷移方程式ｆ（Ｘ）を記憶し、（Ｂ）複数のロボット制御装置２０により、データ記憶装置１６に記憶された最新の内部状態Ｘに基づき、各ロボット１の制御周期で、各ロボット１に必要な予測値を予測し、複数のロボット１をリアルタイムに制御する。
【解決手段】（Ａ）ロボット１の制御周期に依らない任意のタイミングで、計測装置１２、状態推定装置１４、及びデータ記憶装置１６により、対象物やロボット１のセンサ情報を取得し、センサ情報と同時刻の各ロボット１の内部状態Ｘを予測し、センサ情報と比較して内部状態を更新し、更新した内部状態Ｘと予測に用いた状態遷移方程式ｆ（Ｘ）を記憶し、（Ｂ）複数のロボット制御装置２０により、データ記憶装置１６に記憶された最新の内部状態Ｘに基づき、各ロボット１の制御周期で、各ロボット１に必要な予測値を予測し、複数のロボット１をリアルタイムに制御する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットなどを使った自動装置のうち、複数のロボットの運動を予測する運動予測制御方法と装置に関する。
【背景技術】
【０００２】
ロボット上やロボットの外部に設置したセンサ情報を使って、ロボットの位置や姿勢を算出し、ロボットを駆動制御する制御装置において、ロボットの位置と姿勢の計測に使われるセンサや計測方法としては、以下が一般的である。
（１） ロボット上の内界センサを使う方法。
例えば、ロボット上に設置したジャイロセンサや車輪のエンコーダなどで、ロボットの速度、加速度、角速度などを計測する。ロボット上に設置したステアリング部のエンコーダなどで、ロボットのステア角を計測する。
（２） ロボット上の外界センサを使う方法。
例えば、ロボット上に設置した測距センサ（レーザーレンジファインダ（ＬＲＦ）、レーザレーダなど）やカメラなどで、ロボット周囲のランドマークの位置や姿勢を計測する。ロボット上に設置したＧＰＳコンパスなどで、ロボットの現在の位置と姿勢の一方又は両方を計測する。
（３） ロボット外の外部センサで計測する方法。
ロボット外部に設置した測距センサやカメラなどで、計測範囲内のロボットの位置や姿勢を計測する。
【０００３】
単一のセンサでロボットの位置や姿勢を精度よく求めることは困難なため、上述のセンサや計測方法を複数用いて、得られた複数の計測結果から位置や姿勢を求めることがある。
しかし、以下のような理由で、複数の計測結果を簡単に比較することはできない。
（２）（３）で計測される位置と姿勢は、計測するセンサや、特定の点を基点とする座標系で表される相対的な位置と姿勢である。計測座標の基点はそれぞれ異なる上に、ロボット上のセンサの場合は、基点となるロボット自身の位置が不明なため、単純に座標変換等で、これらの計測結果を比較することは難しい。
（１）は、位置や姿勢を直接計測するものではないため、（２）（３）と直接比較できない。
【０００４】
以下のような理由で全てのセンサ情報を同期して計測することは困難である。したがって、得られた計測結果の計測時刻は、それぞれ異なる場合がある。ロボットの位置と姿勢は時々刻々と変化するため、時刻の揃っていない計測結果を単純に比較することはできない。特にロボットの速度が速い場合などでは、位置と姿勢の変化量が大きいため、このような問題が起こりやすい。
【０００５】
ａ 一般的に、同一ロボット内の内界センサのセンサ情報は同期して取得される。しかし、異なるロボット間で、１つの同期信号を共有するのは難しい（有線接続が必要）。したがって、複数のロボット間で内界センサの情報を同期するのは困難である。
ｂ （３）のように外部センサを用いる場合も、ａと同様に、複数の装置間で同期信号を授受する必要があり、外部センサとロボット上のセンサの同期は困難である。
ｃ （２）や（３）で使われるセンサには、センサそのものの計測時間や、センサから得られたセンサ情報を処理する時間などによる計測遅れがある。したがって、センサで得られる計測結果は、計測遅れの分古いセンサ情報となるため、（１）のような内界センサのセンサ情報と計測時刻が異なる。また、計測対象がセンサの視野から外れる場合があるため、指定した時刻の計測結果が常に得られるとは限らない。
【０００６】
そこで上述した問題を解決するために、種々の制御手段が既に提案されている（例えば、特許文献１〜３）。
【０００７】
特許文献１の「３次元運動予測装置」は、単振動を行なう被計測体の位置データからその運動パラメータを推定し、その将来の位置を予測し、その位置情報を基にマニピュレータにより被計測体を把持するものである。
特許文献２の状態推定手段は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するものである。内部状態とは、対象物の位置、姿勢、振れ角などの状態変数を意味する。
特許文献３の「運動予測装置」は、バックグラウンド処理とフォアグラウンド処理を併用して自動追尾と運動予測を行なうものである。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平０７−０１９８１８号公報、「３次元運動予測装置」
【特許文献２】特許第４０７２０１７号公報、「状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置」
【特許文献３】特許第４１５３６２５号公報、「運動予測装置」
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述した特許文献１によれば、対象物（被計測体）を計測した観測履歴から、対象物の運動状態を推定することで、計測値が得られていない間のデータを補完したり、対象物をロボットで把持する際の把持地点を予測したりすることができる。
【００１０】
運動状態の推定には、カルマンフィルタやパーティクルフィルタなどのベイズフィルタが一般的に用いられる。ベイズフィルタでは、特許文献２に開示されているように、運動の状態を表す「内部状態」と、「状態遷移方程式」、「観測方程式」を定義する。
【００１１】
ベイズフィルタは、大きく分けて、以下の２つの処理で実現される。
（１）任意の時刻における内部状態を予測する「予測処理」。
（２）内部状態から計測値を予測し、実際の計測値と比較して、内部状態を修正する「更新処理」（特許文献２では、「濾波推定手段」と記述されている）。
【００１２】
ロボットが状態推定結果を制御に用いる場合、以下の２通りの構成が考えられる。
Ａ ロボット制御装置内に上記のアルゴリズムが実装され、ロボット制御周期ごとに（１）の予測処理を行ない、センサの計測値が得られた時に、（２）の更新処理を行なう。
Ｂ 特許文献１のように、状態推定装置とロボットで構成されるシステムとし、ロボットが推定装置から予測値を読み出して制御に用いる。
【００１３】
ロボットなどの自動機械の運動を制御する際、一定の制御周期（例えば４ｍｓの周期）で制御指令値を算出することが求められる。これに対し従来技術では、運動状態の推定を使うシステムを、時間制約を考慮して構成する具体的な方法が提案されていなかった。
【００１４】
例えば上述したＡの手段では、ロボットの制御周期内に更新処理が終わらない可能性がある。状態推定器の更新処理には、一般的に時間がかかり、特に計測値の種類が多い場合、パーティクルフィルタや、その他の複雑な処理を使う場合には特に時間がかかる。
また、特許文献２の手段は、観測値や状態変数の種類に応じて予測処理、更新処理を分けることで、目的に応じて計算量を減らすことができるが、複雑な運動や、得られる計測値を全て反映させたい場合は、結局全ての処理を連結する必要があるため、推定に必要な計算量は減少しない。
【００１５】
また上述したＢの手段は、ロボット制御装置が更新処理をする必要はないが、ロボットが予測値を要求してから、推定装置が一定時間内に予測処理を完了し、予測値を返答しなければならない。これを満たす状態推定装置を設計するには、次のような問題を考慮する必要がある。
【００１６】
（１）ロボットが多数に増えた時や、更新処理の負荷が高い時は、推定装置の演算処理能力が分散される。このような状況でも、予測処理は一定時間内に完了しなければならない。
（２）ロボットの制御周期はロボットごとに異なるため、ロボット側の要求から予測値を返すまでの時間制約は、一意に決まらない。
（３）ロボット制御装置と状態推定装置間の通信に、一定の時間がかかる。この通信の所要時間は、通信方式（有線、無線など）によって異なる。また、ロボットが多数ある場合、ロボットと状態推定装置間の通信負荷は、状況によって変わる。したがって、通信所要時間も変わる。
【００１７】
複数のロボットが同一エリア内で稼動する場合は、ロボット同士がぶつからないようするなど、互いの相対的な運動（相対的な位置と姿勢や、その変化）に基づいて制御する。このようにロボットや外部センサが複数ある場合は、特に、上述のような問題が起こりやすい。
【００１８】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、状態推定の計算量やデータ通信の所要時間の影響を受けることなく、ロボットごとに決められた制御周期で制御指令値を算出し、複数のロボットを制御することができるロボットの運動予測制御方法と装置を提供することにある。
【課題を解決するための手段】
【００１９】
本発明によれば、（Ａ）ロボットの制御周期に依らない任意のタイミングで、
計測装置により対象物や複数のロボットのセンサ情報を取得し、
状態推定装置によりセンサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、
データ記憶装置により、更新した内部状態と予測に用いた状態遷移方程式を記憶し、
（Ｂ）複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する、ことを特徴とするロボットの運動予測制御方法が提供される。
【００２０】
また本発明によれば、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得する計測装置と、
前記センサ情報を取得したときに、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新する状態推定装置と、
前記更新が行われたときに、更新した内部状態と予測に用いた状態遷移方程式を記憶するデータ記憶装置と、
データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する複数のロボット制御装置とを備えた、ことを特徴とするロボットの運動予測制御装置が提供される。
【発明の効果】
【００２１】
上記本発明の方法と装置によれば、計測装置、状態推定装置、及びデータ記憶装置により、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得し、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、更新した内部状態と予測に用いた状態遷移方程式を記憶するので、対象物（例えばランドマーク）やロボットのセンサ情報（例えば位置、姿勢）に基づいてロボットの運動予測をすることができる。
【００２２】
また、複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測するので、状態推定の計算量や、データ通信の所要時間の影響を受けることなく、ロボットごとに決められた制御周期で制御指令値を算出し、複数のロボットを制御することができる。
【００２３】
すなわち、各ロボット制御装置は、状態推定の更新処理にかかる時間の影響を受けることなく動作できる。
また、計測装置やロボット制御装置が増え、状態推定の計算量が増加する場合も、各ロボット制御装置が独立して予測計算を行なうため、予測処理にかかる時間は長くならない。したがって、システム変更の際に、演算処理能力などの設計を見直す必要がなくなる。
さらに、予測処理の時間制約をロボット制御装置ごとに設定できる。また、算出する予測値の精度や、予測値の種類なども、ロボット制御装置ごとに設定できる。したがって、予測処理にかかる時間や制御周期等を考慮して、予測計算の精度を変えたり、内部状態のうち必要な変数のみを計算したりといった工夫を、それぞれのロボットで実現できる。
【００２４】
また、データ記憶装置が、ロボットからの要求に依らないタイミングで、データ記憶装置に記憶された最新の内部状態をロボット上記憶部に転送することで、ロボット制御装置と状態推定装置間の通信に時間がかかる場合や、通信所要時間が一定でない場合でも、予測処理に必要なデータをすぐに参照できる。この場合、ロボット上で参照されるデータは、通信遅れの影響で、必ずしも最新値とならないが、予測処理が一定時間内に完了することができる。
【図面の簡単な説明】
【００２５】
【図１】本発明による運動予測制御装置を備えたロボットシステムの模式図である。
【図２】本発明による運動予測制御装置の第１実施形態図である。
【図３】本発明による運動予測制御装置の第２実施形態図である。
【図４】本発明による運動予測制御装置の第３実施形態図である。
【図５】本発明による運動予測制御装置の第４実施形態図である。
【発明を実施するための形態】
【００２６】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２７】
図１は、本発明による運動予測制御装置を備えたロボットシステムの模式図である。
この図において、１は移動ロボット、２は移動ロボット１の外部に固定されたランドマーク、３は移動ロボット１に固定された内部センサ、４は移動ロボット１の外部に固定された外部センサである。
このシステムにおいて、複数の移動ロボット１は、それぞれ自律的に移動できるようになっている。
以下、移動ロボットを単に「ロボット」と呼ぶ。
【００２８】
図２は、本発明による運動予測制御装置の第１実施形態図である。
この図において、本発明の運動予測制御装置１０は、計測装置１２、状態推定装置１４、データ記憶装置１６、及び複数のロボット制御装置２０を備える。装置１２、１４、２０は、データ記憶装置１６にそれぞれ直接接続されている。
複数のロボット制御装置２０は、この例では第１ロボット制御装置２０Ａ、第２ロボット制御装置２０Ｂ、および第３ロボット制御装置２０Ｃからなる。
なお、ロボット制御装置２０は、この例では３台であるが、２台でも、４台以上であってもよい。
【００２９】
計測装置１２は、対応する外部センサ４に接続され、外部センサ４のセンサ情報（観測値）を取り込む。外部センサ４としては、カメラ、レーザーレンジファインダ（ＬＲＦ）、３次元レーザレーダ、ジャイロセンサ、ＧＰＳコンパス、パルスエンコーダ、などが挙げられる。
計測装置１２は、任意のタイミングで、外部センサ４からセンサ情報を読み込み、後述する計測処理（作用の（１）〜（３））を行なう。
ロボット制御装置２０上の計測部２４は、計測装置１２と同様の機能を有する。また、後述するロボット制御装置２０の構成のうち、移動機構や駆動制御のための処理部が無いものを、この例では「計測装置１２」とする。
【００３０】
状態推定装置１４には、カルマンフィルタのアルゴリズムを基にした状態推定アルゴリズムが実装されている。このアルゴリズムでは、ロボット１の位置ｘ，ｙ，ｚ、速度ｖｘ，ｖｙ，ｖｚ、角速度ｄｒｘ，ｄｒｙ，ｄｒｚなどの状態変数をまとめて、内部状態Ｘとする。
また、時刻が進むと内部状態Ｘがどのように変化するかを示す状態遷移方程式Ｘ（ｔ＋Δｔ）＝ｆ（Ｘ（ｔ））と、内部状態Ｘと観測値Ｙを対応づける観測方程式Ｙ＝ｈ（Ｘ）を定義する。観測方程式は、観測値Ｙ（センサ情報）の種類ごとにそれぞれ定義される。
またこのアルゴリズムは、内部状態Ｘの誤差分布ＣｏｖＸや、観測値Ｙの誤差分布ＣｏｖＹなどを管理しており、これらのデータから状態推定の精度を求めることができる。なお以下の例では、これらの精度指標値をまとめてＥとする。
【００３１】
状態推定装置１４には、全体予測部１５ａと更新部１５ｂの２つの機能が実装される。
【００３２】
全体予測部１５ａは、データ記憶装置１６に記録されている内部状態Ｘを、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行なう。
更新部１５ｂは、全体予測部１５ａを使って計測時刻まで遷移させた内部状態Ｘと、観測方程式ｈ（Ｘ）を使って、対象物（ロボット１又はランドマーク２）の観測値Ｙの予測値を算出する。入力された観測値Ｙと、予測値を比較して、内部状態Ｘを更新し、データ記憶装置１６に記録する。この時、状態推定の精度指標値Ｅも同時に更新し、データ記憶装置１６に記録する。
【００３３】
図２のロボット制御装置２０は、一般的なロボット１で用いられる構成を示している。
この図において、ロボット制御装置２０は、移動のための駆動部２１、駆動部２１を制御する車両制御部２２、ロボット１の目標軌道を算出する行動計画部２３、各センサ３のセンサ処理を行なう計測部２４で構成される。
センサ３として用いられるのは、主に、ロボット１の角速度又は加速度を計測するジャイロセンサ３ａ、周囲の形状を計測する測距センサ３ｂ（ＬＲＦ）、ロボット１に設けられた車輪の回転量を計測する車輪用エンコーダ３ｃである。
【００３４】
図３は、本発明による運動予測制御装置の第２実施形態図である。なおこの図では、ロボット制御装置２０のみを示している。
本発明では、センサ３の種類は限定されない。すなわち例えば図３に示すように、カメラ３ｄ、ＧＰＳコンパス３ｅ、ロボット１に設けられた車輪のステア角計測用のステア角エンコーダ３ｆなどのセンサを追加しても良いし、一部のセンサを使わない構成としても良い。
【００３５】
図２、図３において、行動計画部２３は、一定の制御周期で動作するリアルタイム処理系であり、ロボット上予測部２３ａを使って、自ロボット１の現在又は未来の位置や姿勢を予測し、軌道生成部２３ｂによるロボット１の目標軌道の算出に用いる。また、衝突回避の場合など、必要に応じて、他ロボット１の現在又は未来の位置や姿勢を予測する。
【００３６】
ロボット上予測部２３ａは、データ記憶装置１６に記録されている内部状態Ｘのうち、ロボット制御に関連する状態量を、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行なう。ロボット上予測部２３ａは、リアルタイム制御の時間制約下で予測処理できるように設計される。
【００３７】
車両制御部２２は、一定の制御周期で動作するリアルタイム処理系であり、ロボット１が目標軌道に沿って動くように、指令値算出部２２ａで指令値を算出し駆動部２１を制御する。
【００３８】
通信部２５は、ロボット１とデータ記憶装置１６間のデータ通信機能を有する。具体的には、イーサネット（登録商標）やＵＳＢなどの有線通信や、Ｂｌｕｅｔｏｏｔｈ、無線ＬＡＮ、などの無線通信を行なうデバイスを用いる。
【００３９】
データ記憶装置１６は、内部状態Ｘ、内部状態のモデル時刻ｔｘ、観測値Ｙ、計測された時刻ｔｙ、状態遷移方程式ｆ、観測方程式ｈ、状態推定の精度指標値Ｅなどのデータを保持する。
【００４０】
以下、上述した本発明の運動予測制御装置１０の作用を説明する。
計測装置１２または計測部２４は、以下の（１）〜（３）を任意のタイミングで繰り返す。
（１）任意のタイミングでセンサ情報を取得する。例えばカメラの場合は、カメラにシャッター信号を送信し、撮像された画像を取得する。
（２）取得したセンサ情報を処理し、ロボット１やランドマーク２の位置と姿勢や、ロボット１の速度又は角速度などの、観測値Ｙを求める。観測値Ｙは、センサごとに異なるため、センサごとに定義される。また、ここで行なうセンサ情報の処理も、センサの種類によって異なる。例えばカメラの場合は、得られた画像から画像処理によって対象物（ロボット１又はランドマーク２）の位置と姿勢のセンサ情報を取得する（例：画像内の白色領域を抽出し、重心を求める）。また例えば、パルスエンコーダの場合はパルス波のカウント処理などが考えられる。
（３）算出した観測値Ｙと、計測された時刻ｔｙ（シャッター時刻など）、観測値Ｙの誤差分布ＣｏｖＹをデータ記憶装置１６に記録する。
【００４１】
また、状態推定装置１４は以下の（４）〜（６）を観測値Ｙが更新されるたびに実施する。
（４）データ記憶装置１６を監視し、新たな観測値Ｙが記録されていたら、データ記憶装置１６からＹ，ｔｙ，Ｘ，ｔｘ，ｆ，ｈ，Ｅを読み出す。
（５）内部状態Ｘは、時刻ｔｘの時の内部状態を示しているので、全体予測部１５ａを使って、時刻ｔｙのときの内部状態を予測する。
（６）更新部１５ｂを使って、（５）で予測した内部状態から時刻ｔｙの時の観測値を予測し、予測値と観測値Ｙを比較して、内部状態Ｘを修正する。修正した内部状態Ｘと新たなモデル時刻ｔｘ（＝ｔｙ）をデータ記憶装置１６に記録する。また更新部１５ｂは、内部状態Ｘの誤差分布ＣｏｖＸや、観測値と予測値のずれの大きさなどの精度指標値Ｅを算出し、データ記憶装置１６に記録する。
【００４２】
また、ロボット制御装置２０上の行動計画部２３は、以下の（７）〜（８）を一定の周期で繰り返す。
（７）ロボット上予測部２３ａを使って自ロボット１の現在位置や未来の位置を算出する。また、衝突回避などの必要に応じて、他ロボット１の現在位置や未来の位置を算出する。
（８）自ロボット１が目標位置に移動するような目標軌道を生成する。
【００４３】
また、ロボット制御装置２０上の車両制御部２２は、以下の（９）〜（１１）を一定の周期で繰り返す。
（９）車輪用エンコーダ３ｃから自ロボット１の現在速度を求める。
（１０）行動計画部２３で生成された目標軌道に沿って動くような、制御指令値を算出する。
（１１）駆動部２１に算出した制御指令値を送信する。
【００４４】
駆動部２１を精密に制御するには、車両制御部２２が一定周期で制御指令値を算出する必要がある。また、車両制御部２２で参照する目標軌道は、行動計画部２３で事前に生成されている必要がある。したがって、行動計画部２３、車両制御部２２の処理は、一定の周期で繰り返されるリアルタイム処理である。
【００４５】
一般に、行動計画部２３の制御周期は、車両制御部２２の制御周期よりも長い（一度に生成する目標軌道の長さなどによる）。また、目標軌道には、車両制御部２２が参照する直近の移動経路の他に、目標地点までの大まかな移動経路などがある。したがって、局所の経路生成、大域の経路生成、より大域の経路生成のように、行動計画部２３をさらに２つ以上の処理に分けることもある。この場合は、局所の経路を生成する処理の方が、短い制御周期となる。大域の方は、長い制御周期か、周期が一定でない非リアルタイムな処理でも構わない。
【００４６】
図４は、本発明による運動予測制御装置の第３実施形態図である。この例において、ロボット制御装置２０は、さらにロボット上記憶部２６を備える。
上述した（７）の計算を行なう際、ロボット上予測部２３ａは、データ記憶装置１６にある最新の内部状態Ｘを参照する。この時、通信のための時間がかかることで、一定の時間制約下で予測値を求められない場合がある。そこで、この例では、データ記憶装置１６の内容を、ロボット上記憶部２６に逐次転送し、ロボット上予測部２３ａは、ロボット上記憶部２６を参照する。転送するタイミングは、データ記憶装置１６の内部状態Ｘが更新された時とする（上述した（６）の直後）。
【００４７】
第２ロボット制御装置２０Ｂ、第３ロボット制御装置２０Ｃも、第１ロボット制御装置２０Ａと同様に、ロボット上予測部２３ａを有する。
なお、ロボット制御装置２０は、この例では３台であるが、２台でも、４台以上であってもよい。
【００４８】
図５は、本発明による運動予測制御装置の第４実施形態図である。なおこの図では、ロボット制御装置２０のみを示している。
上述した例では、説明上「計測装置１２」という表現を用いたが、計測装置１２は移動機構を持たないロボット制御装置２０であり、ロボット制御装置２０の一種として扱う。
したがって、本発明のシステム構成は状態推定装置１４とロボット制御装置２０の２種類とし、ロボット制御装置２０は、移動機構を持たないものを含むとする。
データ記憶装置１６は、複数の装置、プログラムが同一のデータを読み書きする共有メモリであれば良い。したがって、独立した装置とする必要はなく、図５に示すように状態推定装置１４などのメモリ空間上に用意されていれば良い。
また状態推定装置１４は、いずれかのロボット１上に設置され、図５に示すようにロボット制御装置２０と一体とする構成であってもよい。例えば、状態推定プログラム、ロボット制御プログラムとして、一つのＰＣ上で複数プログラムを並列処理させることができる。
【００４９】
上述した例では、ロボット上予測部２３ａで算出した予測値をロボットの制御に用いたが、周囲の人や物体をロボットが計測して、その位置や運動の情報をロボット上予測部２３ａで予測して人に提示するなど、リアルタイムに変化する情報を提示するサービスに用いることもできる。
【００５０】
上述した本発明の方法と装置によれば、計測装置１２、状態推定装置１４、及びデータ記憶装置１６により、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボット１のセンサ情報を取得し、センサ情報と同時刻の各ロボット１の内部状態Ｘを予測して、予測した内部状態をセンサ情報と比較して更新し、更新した内部状態Ｘと予測に用いた状態遷移方程式ｆ（Ｘ）を記憶するので、対象物（例えばランドマーク２）やロボット１のセンサ情報（例えば位置、姿勢）に基づいて各ロボット１の運動予測をすることができる。
【００５１】
また、複数のロボット制御装置２０が、データ記憶装置１６に記憶された最新の内部状態Ｘに基づき、各ロボット１に必要な予測値を予測するので、状態推定の計算量や、データの通信所要時間の影響を受けることなく、ロボット１ごとに決められた制御周期で制御指令値を算出し、複数のロボット１を制御することができる。
【００５２】
すなわち、各ロボット制御装置２０は、状態推定の更新処理にかかる時間の影響を受けることなく動作できる。
また、計測装置１２やロボット制御装置２０が増え、状態推定の計算量が増加する場合も、各ロボット制御装置２０が独立して予測計算を行なうため、予測処理にかかる時間は長くならない。したがって、システム変更の際に、演算処理能力などの設計を見直す必要がなくなる。
さらに、予測処理の時間制約をロボット制御装置２０ごとに設定できる。また、算出する予測値の精度や、予測値の種類なども、ロボット制御装置２０ごとに設定できる。したがって、予測処理にかかる時間や制御周期等を考慮して、予測計算の精度を変えたり、内部状態Ｘのうち必要な変数のみを計算したりといった工夫を、それぞれのロボットで実現できる。
【００５３】
また、データ記憶装置１６が、ロボット１からの要求に依らないタイミングで、データ記憶装置１６に記憶された最新の内部状態Ｘをロボット上記憶部２６に転送することで、ロボット制御装置２０と状態推定装置１４間の通信に時間がかかる場合や、通信所要時間が一定でない場合でも、予測処理に必要なデータをすぐに参照できる。この場合、ロボット上で参照されるデータは、通信遅れの影響で、必ずしも最新値とならないが、予測処理が一定時間内に完了することができる。
【００５４】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【符号の説明】
【００５５】
１ ロボット（自ロボット、他ロボット）、
２ ランドマーク、３ 内部センサ、
３ａ ジャイロセンサ、３ｂ 測距センサ（ＬＲＦ）、
３ｃ 車輪用エンコーダ、３ｄカメラ、３ｅ ＧＰＳコンパス、
３ｆ ステア角エンコーダ、４ 外部センサ、
１０ 運動予測制御装置、１２ 計測装置、１４ 状態推定装置、
１５ａ 全体予測部、１５ｂ 更新部、１６ データ記憶装置、
２０ ロボット制御装置、２０Ａ 第１ロボット制御装置、
２０Ｂ 第２ロボット制御装置、２０Ｃ 第３ロボット制御装置、
２１ 駆動部、２２ 車両制御部、２３ 行動計画部、
２３ａ ロボット上予測部、２３ｂ 軌道生成部、２４ 計測部、
２５ 通信部、２６ ロボット上記憶部
【技術分野】
【０００１】
本発明は、ロボットなどを使った自動装置のうち、複数のロボットの運動を予測する運動予測制御方法と装置に関する。
【背景技術】
【０００２】
ロボット上やロボットの外部に設置したセンサ情報を使って、ロボットの位置や姿勢を算出し、ロボットを駆動制御する制御装置において、ロボットの位置と姿勢の計測に使われるセンサや計測方法としては、以下が一般的である。
（１） ロボット上の内界センサを使う方法。
例えば、ロボット上に設置したジャイロセンサや車輪のエンコーダなどで、ロボットの速度、加速度、角速度などを計測する。ロボット上に設置したステアリング部のエンコーダなどで、ロボットのステア角を計測する。
（２） ロボット上の外界センサを使う方法。
例えば、ロボット上に設置した測距センサ（レーザーレンジファインダ（ＬＲＦ）、レーザレーダなど）やカメラなどで、ロボット周囲のランドマークの位置や姿勢を計測する。ロボット上に設置したＧＰＳコンパスなどで、ロボットの現在の位置と姿勢の一方又は両方を計測する。
（３） ロボット外の外部センサで計測する方法。
ロボット外部に設置した測距センサやカメラなどで、計測範囲内のロボットの位置や姿勢を計測する。
【０００３】
単一のセンサでロボットの位置や姿勢を精度よく求めることは困難なため、上述のセンサや計測方法を複数用いて、得られた複数の計測結果から位置や姿勢を求めることがある。
しかし、以下のような理由で、複数の計測結果を簡単に比較することはできない。
（２）（３）で計測される位置と姿勢は、計測するセンサや、特定の点を基点とする座標系で表される相対的な位置と姿勢である。計測座標の基点はそれぞれ異なる上に、ロボット上のセンサの場合は、基点となるロボット自身の位置が不明なため、単純に座標変換等で、これらの計測結果を比較することは難しい。
（１）は、位置や姿勢を直接計測するものではないため、（２）（３）と直接比較できない。
【０００４】
以下のような理由で全てのセンサ情報を同期して計測することは困難である。したがって、得られた計測結果の計測時刻は、それぞれ異なる場合がある。ロボットの位置と姿勢は時々刻々と変化するため、時刻の揃っていない計測結果を単純に比較することはできない。特にロボットの速度が速い場合などでは、位置と姿勢の変化量が大きいため、このような問題が起こりやすい。
【０００５】
ａ 一般的に、同一ロボット内の内界センサのセンサ情報は同期して取得される。しかし、異なるロボット間で、１つの同期信号を共有するのは難しい（有線接続が必要）。したがって、複数のロボット間で内界センサの情報を同期するのは困難である。
ｂ （３）のように外部センサを用いる場合も、ａと同様に、複数の装置間で同期信号を授受する必要があり、外部センサとロボット上のセンサの同期は困難である。
ｃ （２）や（３）で使われるセンサには、センサそのものの計測時間や、センサから得られたセンサ情報を処理する時間などによる計測遅れがある。したがって、センサで得られる計測結果は、計測遅れの分古いセンサ情報となるため、（１）のような内界センサのセンサ情報と計測時刻が異なる。また、計測対象がセンサの視野から外れる場合があるため、指定した時刻の計測結果が常に得られるとは限らない。
【０００６】
そこで上述した問題を解決するために、種々の制御手段が既に提案されている（例えば、特許文献１〜３）。
【０００７】
特許文献１の「３次元運動予測装置」は、単振動を行なう被計測体の位置データからその運動パラメータを推定し、その将来の位置を予測し、その位置情報を基にマニピュレータにより被計測体を把持するものである。
特許文献２の状態推定手段は、観測により時系列に入力される観測信号に基づいて、その観測を行ったシステムの内部状態を推定するものである。内部状態とは、対象物の位置、姿勢、振れ角などの状態変数を意味する。
特許文献３の「運動予測装置」は、バックグラウンド処理とフォアグラウンド処理を併用して自動追尾と運動予測を行なうものである。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平０７−０１９８１８号公報、「３次元運動予測装置」
【特許文献２】特許第４０７２０１７号公報、「状態推定装置、その方法及びそのプログラム、並びに、現在状態推定装置及び未来状態推定装置」
【特許文献３】特許第４１５３６２５号公報、「運動予測装置」
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述した特許文献１によれば、対象物（被計測体）を計測した観測履歴から、対象物の運動状態を推定することで、計測値が得られていない間のデータを補完したり、対象物をロボットで把持する際の把持地点を予測したりすることができる。
【００１０】
運動状態の推定には、カルマンフィルタやパーティクルフィルタなどのベイズフィルタが一般的に用いられる。ベイズフィルタでは、特許文献２に開示されているように、運動の状態を表す「内部状態」と、「状態遷移方程式」、「観測方程式」を定義する。
【００１１】
ベイズフィルタは、大きく分けて、以下の２つの処理で実現される。
（１）任意の時刻における内部状態を予測する「予測処理」。
（２）内部状態から計測値を予測し、実際の計測値と比較して、内部状態を修正する「更新処理」（特許文献２では、「濾波推定手段」と記述されている）。
【００１２】
ロボットが状態推定結果を制御に用いる場合、以下の２通りの構成が考えられる。
Ａ ロボット制御装置内に上記のアルゴリズムが実装され、ロボット制御周期ごとに（１）の予測処理を行ない、センサの計測値が得られた時に、（２）の更新処理を行なう。
Ｂ 特許文献１のように、状態推定装置とロボットで構成されるシステムとし、ロボットが推定装置から予測値を読み出して制御に用いる。
【００１３】
ロボットなどの自動機械の運動を制御する際、一定の制御周期（例えば４ｍｓの周期）で制御指令値を算出することが求められる。これに対し従来技術では、運動状態の推定を使うシステムを、時間制約を考慮して構成する具体的な方法が提案されていなかった。
【００１４】
例えば上述したＡの手段では、ロボットの制御周期内に更新処理が終わらない可能性がある。状態推定器の更新処理には、一般的に時間がかかり、特に計測値の種類が多い場合、パーティクルフィルタや、その他の複雑な処理を使う場合には特に時間がかかる。
また、特許文献２の手段は、観測値や状態変数の種類に応じて予測処理、更新処理を分けることで、目的に応じて計算量を減らすことができるが、複雑な運動や、得られる計測値を全て反映させたい場合は、結局全ての処理を連結する必要があるため、推定に必要な計算量は減少しない。
【００１５】
また上述したＢの手段は、ロボット制御装置が更新処理をする必要はないが、ロボットが予測値を要求してから、推定装置が一定時間内に予測処理を完了し、予測値を返答しなければならない。これを満たす状態推定装置を設計するには、次のような問題を考慮する必要がある。
【００１６】
（１）ロボットが多数に増えた時や、更新処理の負荷が高い時は、推定装置の演算処理能力が分散される。このような状況でも、予測処理は一定時間内に完了しなければならない。
（２）ロボットの制御周期はロボットごとに異なるため、ロボット側の要求から予測値を返すまでの時間制約は、一意に決まらない。
（３）ロボット制御装置と状態推定装置間の通信に、一定の時間がかかる。この通信の所要時間は、通信方式（有線、無線など）によって異なる。また、ロボットが多数ある場合、ロボットと状態推定装置間の通信負荷は、状況によって変わる。したがって、通信所要時間も変わる。
【００１７】
複数のロボットが同一エリア内で稼動する場合は、ロボット同士がぶつからないようするなど、互いの相対的な運動（相対的な位置と姿勢や、その変化）に基づいて制御する。このようにロボットや外部センサが複数ある場合は、特に、上述のような問題が起こりやすい。
【００１８】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、状態推定の計算量やデータ通信の所要時間の影響を受けることなく、ロボットごとに決められた制御周期で制御指令値を算出し、複数のロボットを制御することができるロボットの運動予測制御方法と装置を提供することにある。
【課題を解決するための手段】
【００１９】
本発明によれば、（Ａ）ロボットの制御周期に依らない任意のタイミングで、
計測装置により対象物や複数のロボットのセンサ情報を取得し、
状態推定装置によりセンサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、
データ記憶装置により、更新した内部状態と予測に用いた状態遷移方程式を記憶し、
（Ｂ）複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する、ことを特徴とするロボットの運動予測制御方法が提供される。
【００２０】
また本発明によれば、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得する計測装置と、
前記センサ情報を取得したときに、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新する状態推定装置と、
前記更新が行われたときに、更新した内部状態と予測に用いた状態遷移方程式を記憶するデータ記憶装置と、
データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する複数のロボット制御装置とを備えた、ことを特徴とするロボットの運動予測制御装置が提供される。
【発明の効果】
【００２１】
上記本発明の方法と装置によれば、計測装置、状態推定装置、及びデータ記憶装置により、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得し、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、更新した内部状態と予測に用いた状態遷移方程式を記憶するので、対象物（例えばランドマーク）やロボットのセンサ情報（例えば位置、姿勢）に基づいてロボットの運動予測をすることができる。
【００２２】
また、複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測するので、状態推定の計算量や、データ通信の所要時間の影響を受けることなく、ロボットごとに決められた制御周期で制御指令値を算出し、複数のロボットを制御することができる。
【００２３】
すなわち、各ロボット制御装置は、状態推定の更新処理にかかる時間の影響を受けることなく動作できる。
また、計測装置やロボット制御装置が増え、状態推定の計算量が増加する場合も、各ロボット制御装置が独立して予測計算を行なうため、予測処理にかかる時間は長くならない。したがって、システム変更の際に、演算処理能力などの設計を見直す必要がなくなる。
さらに、予測処理の時間制約をロボット制御装置ごとに設定できる。また、算出する予測値の精度や、予測値の種類なども、ロボット制御装置ごとに設定できる。したがって、予測処理にかかる時間や制御周期等を考慮して、予測計算の精度を変えたり、内部状態のうち必要な変数のみを計算したりといった工夫を、それぞれのロボットで実現できる。
【００２４】
また、データ記憶装置が、ロボットからの要求に依らないタイミングで、データ記憶装置に記憶された最新の内部状態をロボット上記憶部に転送することで、ロボット制御装置と状態推定装置間の通信に時間がかかる場合や、通信所要時間が一定でない場合でも、予測処理に必要なデータをすぐに参照できる。この場合、ロボット上で参照されるデータは、通信遅れの影響で、必ずしも最新値とならないが、予測処理が一定時間内に完了することができる。
【図面の簡単な説明】
【００２５】
【図１】本発明による運動予測制御装置を備えたロボットシステムの模式図である。
【図２】本発明による運動予測制御装置の第１実施形態図である。
【図３】本発明による運動予測制御装置の第２実施形態図である。
【図４】本発明による運動予測制御装置の第３実施形態図である。
【図５】本発明による運動予測制御装置の第４実施形態図である。
【発明を実施するための形態】
【００２６】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２７】
図１は、本発明による運動予測制御装置を備えたロボットシステムの模式図である。
この図において、１は移動ロボット、２は移動ロボット１の外部に固定されたランドマーク、３は移動ロボット１に固定された内部センサ、４は移動ロボット１の外部に固定された外部センサである。
このシステムにおいて、複数の移動ロボット１は、それぞれ自律的に移動できるようになっている。
以下、移動ロボットを単に「ロボット」と呼ぶ。
【００２８】
図２は、本発明による運動予測制御装置の第１実施形態図である。
この図において、本発明の運動予測制御装置１０は、計測装置１２、状態推定装置１４、データ記憶装置１６、及び複数のロボット制御装置２０を備える。装置１２、１４、２０は、データ記憶装置１６にそれぞれ直接接続されている。
複数のロボット制御装置２０は、この例では第１ロボット制御装置２０Ａ、第２ロボット制御装置２０Ｂ、および第３ロボット制御装置２０Ｃからなる。
なお、ロボット制御装置２０は、この例では３台であるが、２台でも、４台以上であってもよい。
【００２９】
計測装置１２は、対応する外部センサ４に接続され、外部センサ４のセンサ情報（観測値）を取り込む。外部センサ４としては、カメラ、レーザーレンジファインダ（ＬＲＦ）、３次元レーザレーダ、ジャイロセンサ、ＧＰＳコンパス、パルスエンコーダ、などが挙げられる。
計測装置１２は、任意のタイミングで、外部センサ４からセンサ情報を読み込み、後述する計測処理（作用の（１）〜（３））を行なう。
ロボット制御装置２０上の計測部２４は、計測装置１２と同様の機能を有する。また、後述するロボット制御装置２０の構成のうち、移動機構や駆動制御のための処理部が無いものを、この例では「計測装置１２」とする。
【００３０】
状態推定装置１４には、カルマンフィルタのアルゴリズムを基にした状態推定アルゴリズムが実装されている。このアルゴリズムでは、ロボット１の位置ｘ，ｙ，ｚ、速度ｖｘ，ｖｙ，ｖｚ、角速度ｄｒｘ，ｄｒｙ，ｄｒｚなどの状態変数をまとめて、内部状態Ｘとする。
また、時刻が進むと内部状態Ｘがどのように変化するかを示す状態遷移方程式Ｘ（ｔ＋Δｔ）＝ｆ（Ｘ（ｔ））と、内部状態Ｘと観測値Ｙを対応づける観測方程式Ｙ＝ｈ（Ｘ）を定義する。観測方程式は、観測値Ｙ（センサ情報）の種類ごとにそれぞれ定義される。
またこのアルゴリズムは、内部状態Ｘの誤差分布ＣｏｖＸや、観測値Ｙの誤差分布ＣｏｖＹなどを管理しており、これらのデータから状態推定の精度を求めることができる。なお以下の例では、これらの精度指標値をまとめてＥとする。
【００３１】
状態推定装置１４には、全体予測部１５ａと更新部１５ｂの２つの機能が実装される。
【００３２】
全体予測部１５ａは、データ記憶装置１６に記録されている内部状態Ｘを、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行なう。
更新部１５ｂは、全体予測部１５ａを使って計測時刻まで遷移させた内部状態Ｘと、観測方程式ｈ（Ｘ）を使って、対象物（ロボット１又はランドマーク２）の観測値Ｙの予測値を算出する。入力された観測値Ｙと、予測値を比較して、内部状態Ｘを更新し、データ記憶装置１６に記録する。この時、状態推定の精度指標値Ｅも同時に更新し、データ記憶装置１６に記録する。
【００３３】
図２のロボット制御装置２０は、一般的なロボット１で用いられる構成を示している。
この図において、ロボット制御装置２０は、移動のための駆動部２１、駆動部２１を制御する車両制御部２２、ロボット１の目標軌道を算出する行動計画部２３、各センサ３のセンサ処理を行なう計測部２４で構成される。
センサ３として用いられるのは、主に、ロボット１の角速度又は加速度を計測するジャイロセンサ３ａ、周囲の形状を計測する測距センサ３ｂ（ＬＲＦ）、ロボット１に設けられた車輪の回転量を計測する車輪用エンコーダ３ｃである。
【００３４】
図３は、本発明による運動予測制御装置の第２実施形態図である。なおこの図では、ロボット制御装置２０のみを示している。
本発明では、センサ３の種類は限定されない。すなわち例えば図３に示すように、カメラ３ｄ、ＧＰＳコンパス３ｅ、ロボット１に設けられた車輪のステア角計測用のステア角エンコーダ３ｆなどのセンサを追加しても良いし、一部のセンサを使わない構成としても良い。
【００３５】
図２、図３において、行動計画部２３は、一定の制御周期で動作するリアルタイム処理系であり、ロボット上予測部２３ａを使って、自ロボット１の現在又は未来の位置や姿勢を予測し、軌道生成部２３ｂによるロボット１の目標軌道の算出に用いる。また、衝突回避の場合など、必要に応じて、他ロボット１の現在又は未来の位置や姿勢を予測する。
【００３６】
ロボット上予測部２３ａは、データ記憶装置１６に記録されている内部状態Ｘのうち、ロボット制御に関連する状態量を、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行なう。ロボット上予測部２３ａは、リアルタイム制御の時間制約下で予測処理できるように設計される。
【００３７】
車両制御部２２は、一定の制御周期で動作するリアルタイム処理系であり、ロボット１が目標軌道に沿って動くように、指令値算出部２２ａで指令値を算出し駆動部２１を制御する。
【００３８】
通信部２５は、ロボット１とデータ記憶装置１６間のデータ通信機能を有する。具体的には、イーサネット（登録商標）やＵＳＢなどの有線通信や、Ｂｌｕｅｔｏｏｔｈ、無線ＬＡＮ、などの無線通信を行なうデバイスを用いる。
【００３９】
データ記憶装置１６は、内部状態Ｘ、内部状態のモデル時刻ｔｘ、観測値Ｙ、計測された時刻ｔｙ、状態遷移方程式ｆ、観測方程式ｈ、状態推定の精度指標値Ｅなどのデータを保持する。
【００４０】
以下、上述した本発明の運動予測制御装置１０の作用を説明する。
計測装置１２または計測部２４は、以下の（１）〜（３）を任意のタイミングで繰り返す。
（１）任意のタイミングでセンサ情報を取得する。例えばカメラの場合は、カメラにシャッター信号を送信し、撮像された画像を取得する。
（２）取得したセンサ情報を処理し、ロボット１やランドマーク２の位置と姿勢や、ロボット１の速度又は角速度などの、観測値Ｙを求める。観測値Ｙは、センサごとに異なるため、センサごとに定義される。また、ここで行なうセンサ情報の処理も、センサの種類によって異なる。例えばカメラの場合は、得られた画像から画像処理によって対象物（ロボット１又はランドマーク２）の位置と姿勢のセンサ情報を取得する（例：画像内の白色領域を抽出し、重心を求める）。また例えば、パルスエンコーダの場合はパルス波のカウント処理などが考えられる。
（３）算出した観測値Ｙと、計測された時刻ｔｙ（シャッター時刻など）、観測値Ｙの誤差分布ＣｏｖＹをデータ記憶装置１６に記録する。
【００４１】
また、状態推定装置１４は以下の（４）〜（６）を観測値Ｙが更新されるたびに実施する。
（４）データ記憶装置１６を監視し、新たな観測値Ｙが記録されていたら、データ記憶装置１６からＹ，ｔｙ，Ｘ，ｔｘ，ｆ，ｈ，Ｅを読み出す。
（５）内部状態Ｘは、時刻ｔｘの時の内部状態を示しているので、全体予測部１５ａを使って、時刻ｔｙのときの内部状態を予測する。
（６）更新部１５ｂを使って、（５）で予測した内部状態から時刻ｔｙの時の観測値を予測し、予測値と観測値Ｙを比較して、内部状態Ｘを修正する。修正した内部状態Ｘと新たなモデル時刻ｔｘ（＝ｔｙ）をデータ記憶装置１６に記録する。また更新部１５ｂは、内部状態Ｘの誤差分布ＣｏｖＸや、観測値と予測値のずれの大きさなどの精度指標値Ｅを算出し、データ記憶装置１６に記録する。
【００４２】
また、ロボット制御装置２０上の行動計画部２３は、以下の（７）〜（８）を一定の周期で繰り返す。
（７）ロボット上予測部２３ａを使って自ロボット１の現在位置や未来の位置を算出する。また、衝突回避などの必要に応じて、他ロボット１の現在位置や未来の位置を算出する。
（８）自ロボット１が目標位置に移動するような目標軌道を生成する。
【００４３】
また、ロボット制御装置２０上の車両制御部２２は、以下の（９）〜（１１）を一定の周期で繰り返す。
（９）車輪用エンコーダ３ｃから自ロボット１の現在速度を求める。
（１０）行動計画部２３で生成された目標軌道に沿って動くような、制御指令値を算出する。
（１１）駆動部２１に算出した制御指令値を送信する。
【００４４】
駆動部２１を精密に制御するには、車両制御部２２が一定周期で制御指令値を算出する必要がある。また、車両制御部２２で参照する目標軌道は、行動計画部２３で事前に生成されている必要がある。したがって、行動計画部２３、車両制御部２２の処理は、一定の周期で繰り返されるリアルタイム処理である。
【００４５】
一般に、行動計画部２３の制御周期は、車両制御部２２の制御周期よりも長い（一度に生成する目標軌道の長さなどによる）。また、目標軌道には、車両制御部２２が参照する直近の移動経路の他に、目標地点までの大まかな移動経路などがある。したがって、局所の経路生成、大域の経路生成、より大域の経路生成のように、行動計画部２３をさらに２つ以上の処理に分けることもある。この場合は、局所の経路を生成する処理の方が、短い制御周期となる。大域の方は、長い制御周期か、周期が一定でない非リアルタイムな処理でも構わない。
【００４６】
図４は、本発明による運動予測制御装置の第３実施形態図である。この例において、ロボット制御装置２０は、さらにロボット上記憶部２６を備える。
上述した（７）の計算を行なう際、ロボット上予測部２３ａは、データ記憶装置１６にある最新の内部状態Ｘを参照する。この時、通信のための時間がかかることで、一定の時間制約下で予測値を求められない場合がある。そこで、この例では、データ記憶装置１６の内容を、ロボット上記憶部２６に逐次転送し、ロボット上予測部２３ａは、ロボット上記憶部２６を参照する。転送するタイミングは、データ記憶装置１６の内部状態Ｘが更新された時とする（上述した（６）の直後）。
【００４７】
第２ロボット制御装置２０Ｂ、第３ロボット制御装置２０Ｃも、第１ロボット制御装置２０Ａと同様に、ロボット上予測部２３ａを有する。
なお、ロボット制御装置２０は、この例では３台であるが、２台でも、４台以上であってもよい。
【００４８】
図５は、本発明による運動予測制御装置の第４実施形態図である。なおこの図では、ロボット制御装置２０のみを示している。
上述した例では、説明上「計測装置１２」という表現を用いたが、計測装置１２は移動機構を持たないロボット制御装置２０であり、ロボット制御装置２０の一種として扱う。
したがって、本発明のシステム構成は状態推定装置１４とロボット制御装置２０の２種類とし、ロボット制御装置２０は、移動機構を持たないものを含むとする。
データ記憶装置１６は、複数の装置、プログラムが同一のデータを読み書きする共有メモリであれば良い。したがって、独立した装置とする必要はなく、図５に示すように状態推定装置１４などのメモリ空間上に用意されていれば良い。
また状態推定装置１４は、いずれかのロボット１上に設置され、図５に示すようにロボット制御装置２０と一体とする構成であってもよい。例えば、状態推定プログラム、ロボット制御プログラムとして、一つのＰＣ上で複数プログラムを並列処理させることができる。
【００４９】
上述した例では、ロボット上予測部２３ａで算出した予測値をロボットの制御に用いたが、周囲の人や物体をロボットが計測して、その位置や運動の情報をロボット上予測部２３ａで予測して人に提示するなど、リアルタイムに変化する情報を提示するサービスに用いることもできる。
【００５０】
上述した本発明の方法と装置によれば、計測装置１２、状態推定装置１４、及びデータ記憶装置１６により、ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボット１のセンサ情報を取得し、センサ情報と同時刻の各ロボット１の内部状態Ｘを予測して、予測した内部状態をセンサ情報と比較して更新し、更新した内部状態Ｘと予測に用いた状態遷移方程式ｆ（Ｘ）を記憶するので、対象物（例えばランドマーク２）やロボット１のセンサ情報（例えば位置、姿勢）に基づいて各ロボット１の運動予測をすることができる。
【００５１】
また、複数のロボット制御装置２０が、データ記憶装置１６に記憶された最新の内部状態Ｘに基づき、各ロボット１に必要な予測値を予測するので、状態推定の計算量や、データの通信所要時間の影響を受けることなく、ロボット１ごとに決められた制御周期で制御指令値を算出し、複数のロボット１を制御することができる。
【００５２】
すなわち、各ロボット制御装置２０は、状態推定の更新処理にかかる時間の影響を受けることなく動作できる。
また、計測装置１２やロボット制御装置２０が増え、状態推定の計算量が増加する場合も、各ロボット制御装置２０が独立して予測計算を行なうため、予測処理にかかる時間は長くならない。したがって、システム変更の際に、演算処理能力などの設計を見直す必要がなくなる。
さらに、予測処理の時間制約をロボット制御装置２０ごとに設定できる。また、算出する予測値の精度や、予測値の種類なども、ロボット制御装置２０ごとに設定できる。したがって、予測処理にかかる時間や制御周期等を考慮して、予測計算の精度を変えたり、内部状態Ｘのうち必要な変数のみを計算したりといった工夫を、それぞれのロボットで実現できる。
【００５３】
また、データ記憶装置１６が、ロボット１からの要求に依らないタイミングで、データ記憶装置１６に記憶された最新の内部状態Ｘをロボット上記憶部２６に転送することで、ロボット制御装置２０と状態推定装置１４間の通信に時間がかかる場合や、通信所要時間が一定でない場合でも、予測処理に必要なデータをすぐに参照できる。この場合、ロボット上で参照されるデータは、通信遅れの影響で、必ずしも最新値とならないが、予測処理が一定時間内に完了することができる。
【００５４】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【符号の説明】
【００５５】
１ ロボット（自ロボット、他ロボット）、
２ ランドマーク、３ 内部センサ、
３ａ ジャイロセンサ、３ｂ 測距センサ（ＬＲＦ）、
３ｃ 車輪用エンコーダ、３ｄカメラ、３ｅ ＧＰＳコンパス、
３ｆ ステア角エンコーダ、４ 外部センサ、
１０ 運動予測制御装置、１２ 計測装置、１４ 状態推定装置、
１５ａ 全体予測部、１５ｂ 更新部、１６ データ記憶装置、
２０ ロボット制御装置、２０Ａ 第１ロボット制御装置、
２０Ｂ 第２ロボット制御装置、２０Ｃ 第３ロボット制御装置、
２１ 駆動部、２２ 車両制御部、２３ 行動計画部、
２３ａ ロボット上予測部、２３ｂ 軌道生成部、２４ 計測部、
２５ 通信部、２６ ロボット上記憶部
【特許請求の範囲】
【請求項１】
（Ａ）ロボットの制御周期に依らない任意のタイミングで、
計測装置により対象物や複数のロボットのセンサ情報を取得し、
状態推定装置によりセンサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、
データ記憶装置により、更新した内部状態と予測に用いた状態遷移方程式を記憶し、
（Ｂ）複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する、ことを特徴とするロボットの運動予測制御方法。
【請求項２】
前記（Ｂ）において、各ロボットの制御周期で、各ロボットの制御に必要な予測値を予測し、複数のロボットをリアルタイムに制御する、ことを特徴とする請求項１に記載の運動予測制御方法。
【請求項３】
前記制御周期は、計測装置の処理間隔よりも短い一定周期である、ことを特徴とする請求項１に記載の運動予測制御方法。
【請求項４】
ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得する計測装置と、
前記センサ情報を取得したときに、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新する状態推定装置と、
前記更新が行われたときに、更新した内部状態と予測に用いた状態遷移方程式を記憶するデータ記憶装置と、
データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する複数のロボット制御装置とを備えた、ことを特徴とするロボットの運動予測制御装置。
【請求項５】
前記複数のロボット制御装置は、各ロボットの制御周期で、各ロボットの制御に必要な予測値を予測し、複数のロボットをリアルタイムに制御する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項６】
状態推定装置は、前記内部状態を予測する全体予測部と、全体予測部が予測した内部状態を更新する更新部とを有し、
各ロボット制御装置は、各ロボットの制御周期でその制御に必要な予測値を予測するロボット上予測部を有する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項７】
各ロボット制御装置は、データ記憶装置に記憶された最新の内部状態を記憶するロボット上記憶部を有し、
データ記憶装置は、ロボットからの要求に依らないタイミングで、データ記憶装置に記憶された最新の内部状態をロボット上記憶部に転送する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項８】
前記内部状態は、ロボットの位置、姿勢、速度、又は角速度の状態変数である、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項１】
（Ａ）ロボットの制御周期に依らない任意のタイミングで、
計測装置により対象物や複数のロボットのセンサ情報を取得し、
状態推定装置によりセンサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新し、
データ記憶装置により、更新した内部状態と予測に用いた状態遷移方程式を記憶し、
（Ｂ）複数のロボット制御装置により、データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する、ことを特徴とするロボットの運動予測制御方法。
【請求項２】
前記（Ｂ）において、各ロボットの制御周期で、各ロボットの制御に必要な予測値を予測し、複数のロボットをリアルタイムに制御する、ことを特徴とする請求項１に記載の運動予測制御方法。
【請求項３】
前記制御周期は、計測装置の処理間隔よりも短い一定周期である、ことを特徴とする請求項１に記載の運動予測制御方法。
【請求項４】
ロボットの制御周期に依らない任意のタイミングで、対象物や複数のロボットのセンサ情報を取得する計測装置と、
前記センサ情報を取得したときに、センサ情報と同時刻の各ロボットの内部状態を予測して、予測した内部状態をセンサ情報と比較して更新する状態推定装置と、
前記更新が行われたときに、更新した内部状態と予測に用いた状態遷移方程式を記憶するデータ記憶装置と、
データ記憶装置に記憶された最新の内部状態に基づき、各ロボットに必要な予測値を予測する複数のロボット制御装置とを備えた、ことを特徴とするロボットの運動予測制御装置。
【請求項５】
前記複数のロボット制御装置は、各ロボットの制御周期で、各ロボットの制御に必要な予測値を予測し、複数のロボットをリアルタイムに制御する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項６】
状態推定装置は、前記内部状態を予測する全体予測部と、全体予測部が予測した内部状態を更新する更新部とを有し、
各ロボット制御装置は、各ロボットの制御周期でその制御に必要な予測値を予測するロボット上予測部を有する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項７】
各ロボット制御装置は、データ記憶装置に記憶された最新の内部状態を記憶するロボット上記憶部を有し、
データ記憶装置は、ロボットからの要求に依らないタイミングで、データ記憶装置に記憶された最新の内部状態をロボット上記憶部に転送する、ことを特徴とする請求項４に記載の運動予測制御装置。
【請求項８】
前記内部状態は、ロボットの位置、姿勢、速度、又は角速度の状態変数である、ことを特徴とする請求項４に記載の運動予測制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１２−２４７８３５（Ｐ２０１２−２４７８３５Ａ）
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願番号】特願２０１１−１１６８８３（Ｐ２０１１−１１６８８３）
【出願日】平成２３年５月２５日（２０１１．５．２５）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．Ｂｌｕｅｔｏｏｔｈ
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願日】平成２３年５月２５日（２０１１．５．２５）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．Ｂｌｕｅｔｏｏｔｈ
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
[ Back to top ]