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