JP3765713B2 - ロボットの協調制御方法及び協調制御装置 - Google Patents
ロボットの協調制御方法及び協調制御装置 Download PDFInfo
- Publication number
- JP3765713B2 JP3765713B2 JP2000209129A JP2000209129A JP3765713B2 JP 3765713 B2 JP3765713 B2 JP 3765713B2 JP 2000209129 A JP2000209129 A JP 2000209129A JP 2000209129 A JP2000209129 A JP 2000209129A JP 3765713 B2 JP3765713 B2 JP 3765713B2
- Authority
- JP
- Japan
- Prior art keywords
- robot
- target
- force
- trajectory
- target object
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Description
【０００１】
【発明の属する技術分野】
本発明は、対象物体を把持し作業を行う複数のロボットに対する協調制御に関するものである。
【０００２】
【従来の技術】
従来の、複数のロボットにより対象物体を把持して作業を行う協調制御方法として、各ロボットに力センサを装備し、力誤差と位置誤差をフィードバックして手先に任意のインピーダンス特性を保有させる方法が知られている（例えば、小菅一弘、吉田英博、他、”インピーダンス制御に基づく双腕マニピュレータの協調制御、”日本ロボット学会誌、Ｖｏｌ．１３、Ｎｏ．３、ＰＰ．４０４−１４０、１９９５参照）。この方法では、高価な力センサを必要とするため、制御系を構成するためのコストが高く、また、幾何学的誤差がある場合には位置と内力を正確に制御できないことが広く知られている。
【０００３】
また、複数のロボットにより対象物体を把持し作業を行う協調制御方法として、各ロボットに力センサを装着し、力誤差と運動誤差をフィードバックする方法が知られている（例えば、P.R.Pagilla and M.Tomizuka,“Adaptive Control of Two Robot Arms Carrying an Unknown Object,”Proc. IEEE Robotics and Automation,ＰＰ．５９７−６０２，１９９５ 参照）。この方法でも高価でかつ雑音の影響を受けやすい力センサを必要とするため、制御系を構成するためのコストが高く、制御システムの高信頼度化の妨げの要因となっていた。また、各ロボット毎に制御入力の生成ができないため、制御器を実装するコストを引き上げる要因となる。
【０００４】
さらに、二つのロボットのうち、一方のロボットが対象物体を把持し、他方のロボットが把持された対象物体に作業を行う協調制御方式も知られている（例えば、Y. H.Liu,S. Arimoto and K. Kitagaki,“Adaptive Control for holonomically constrained robots:time-invariant and time-variant cases,”Proc. IEEE Robotics and Automation,ＰＰ．９０５−９１２，１９９５ 参照）が、各ロボットとも力センサの装着を必要とするため、制御系のコストが高く、制御システムの高信頼度化を妨げる要因となっていた。
【０００５】
【発明が解決しようとする課題】
本発明の目的は、複数のロボットにより対象物体を把持し作業を行う協調制御において、各ロボットに力センサを必要としない、目標軌道に漸近安定な、並列処理可能な協調制御を提供することを目的としている。
【０００６】
【課題を解決するための手段】
上述の目的を達成するために、複数のロボットが、協調して対象物体を把持し作業を行う協調制御方法において、前記対象物体の動力学パラメータを推定し、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、前記推定された対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、各ロボットは、与えられた前記力と運動の目標軌道から動きを制御することを特徴とする。
上記制御方法は、ロボット目標軌道生成部と、前記複数の各ロボットに対応したロボット制御部とで構成され、協調して対象物体を把持し作業を行う協調制御装置に実装することができる。
各ロボットの動きに対する前記制御は、制御対象のロボットの動力学パラメータを推定し、該ロボットの推定した前記動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道とに基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより行うことができる。
【０００７】
上述の制御を用いることにより、各ロボットの手先に力センサを装備する必要はなく、対象物体を目標とする軌道に漸近的かつ安定に追従させることが可能である。
対象物体に関する動力学パラメータ推定、および各ロボットの動力学パラメータ推定と制御入力の計算は、すべて独立に行うことができ、複数の制御器や計算機に計算負荷を分配させることができる。これにより、複数のロボットによる協調制御が、高価な制御システムを構成することなく実現が可能となる。このとき、各ロボットは完全に独立して制御でき、各ロボット制御装置間の通信は必要とはしない。
さらに、対象物体の目標軌道は作業座標系で記述されるため、ロボットの関節変位で対象物体の目標軌道を記述する必要がなく、実際の現揚で目標軌道を生成することが容易である。
【０００８】
また、ロボットの動力学パラメータが既知の場合は、各ロボットの動きに対する前記制御は、該ロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより行うこともできる。このように、各ロボットの動力学パラメータが既知のとき、各ロボットの動力学のパラメータ推定を省略し、既知の値を用いてフィードフォーワード補償項を構成することができる。この場合、対象物体の位置姿勢のみでなく、対象物体の内部に生ずる内力を、力センサを用いなくとも目標内力に収束させることができる。
【０００９】
対象物体の動力学パラメータとロボットの動力学パラメータが既知の場合は、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、各ロボットは、該ロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより、与えられた前記力と運動の目標軌道から動きを制御することもできる。
【００１０】
このように、対象物体やロボットの動力学パラメータが既知のとき、ロボットおよび対象物体の動力学パラメータの推定を省略し、既知の値を用いてフィードフォーワード補償項を構成することができる。この場合も、対象物体の位置姿勢とともに、対象物体内部に生ずる内力を、力センサを用いずに、目標内力に漸近的に収束させることができる。
【００１１】
【発明の実施の形態】
本発明の実施形態を、図面を参照して詳細に説明する。
図１に示すような複数のロボット（アームまたは指）による物体把持を考える。図１で用いる記号を以下に示す。
【数１】
さて、上述の複数のロボット（アーム）による物体把持に関して、以下の条件を設ける。
（１）物体を把持するアームはｋ本である。
（２）アームの手先と物体表面の接触は摩擦のある接触、または、ハンドにより堅く把持され、手先に任意の力とモーメントを生じさせることができる。
（３）物体座標系から観測して、接触点は運動をしない。
（４）アームの手先は接触点から離れることはない、すなわち、Ｐi＝Ｐci
（５）ｐociは計測できる。
【００１２】
＜把持物体の運動方程式＞
把持物体（対象物体）は、一般に次の運動方程式によって記述される。
【数２】
ここで、Ｍo，Ｃo，ｇoはそれぞれ、把持物体の質量行列，粘性行列，および重力項であり、
【数３】
は把持物体に設定された、物体座標系をタスク座標系で観測したときの原点位置ベクトルｐoと姿勢ベクトルηoからなる位置姿勢ベクトルで、Ｆoはタスク座標系で観測した、把持物体の物体座標系原点に働く外力である。
【００１３】
＜把持行列＞
外力ベクトルＦoを、
【数４】
とおく。ｆoは並進力成分で、ｎoはモーメント成分である。また、タスク座標系で観測した、ｉ番目の接触点に働く力とモーメントを、それぞれｆi，ｎiとおき、これらからなる接触力ベクトルを
【数５】
とおく。このとき、接触力Ｆiと把持物体の物体座標系原点に働く力Ｆoの間には以下の関係式が成り立つ。
【数６】
上式を行列を用いて記述すると以下のように書ける。
【数７】
で定義される３×３歪対称行列である。
【００１４】
＜アームｉの運動方程式＞
タスク空間で記述したアームｉの運動方程式が、
【数８】
として記述されるものとする。ここで、Ｍi，Ｃi，ｇiはそれぞれ、アームｉの慣性テンソル、粘性行列および重力項であり、ｒiはアームｉの手先に設定された手先座標系の位置姿勢ベクトルである。また、Ｆiはアームｉの手先に働く力で、ｕiは制御入力である。
【００１５】
本発明における適応協調制御は、タスク座標系で与えられた把持物体の目標軌道と、物体内部に発生させる目標内力ｆint dに対して、
（１）把持物体に目標となる運動を生じさせ、また目標内力を生じさせるための、把持物体に加えるべき目標外力を把持物体の参照モデルを使って計算し、
（２）得られた把持物体の目標外力から各アームの手先の力目標値と目標軌道を計算し、それらを各アームのコントローラに分配し、
（３）アームのコントローラによって、把持物体に目標とする運動を生じさせるように、手先の運動と手先力を適応制御を使って各アームを制御する。
というものである。この適応制御の特徴は、力制御は力のフィードフォーワードのみで、力フィードバックを用いないため、力の計測が不要であること、および把持物体とロボットの動力学パラメータが未知であっても、把持物体を目標軌道に漸近的、かつ安定に追従させることができることにある。以下に詳しく述べる。
【００１６】
＜把持物体に対する適応則＞
把持物体の参照モデルを以下のように定義する。
【数９】
ここでＹoは把持物体のリグレッサで、σoは把持物体の動力学パラメータベクトルである。ｄｒor／ｄｔは、参照速度とよばれ、次式で定義される。
【数１０】
ここで、把持物体の参照モデルを利用して、把持物体に与える目標力Ｆo d（これはアームによって発生される）を次式で与えるものとする。
【数１１】
把持物体のパラメータ適応則には次式を用いる。
【数１２】
ここで、Γoは正定行列である。
＜内力の目標値とアームの目標手先力＞
各アームが発生する手先力Ｆiからなる手先力ベクトルを、
【数１３】
として定義する。ただし、物体はｋ本のアームによって把持されるものとし、Ｆiはｉ番目のアームの手先力である。また、ｉ番目のアームの把持行列を要素とする全体の把持行列
【数１４】
を定義する。このとき、（６）式より、把持物体に働く力は、次式のように与えられる。
【数１５】
上式を手先力ベクトルについて解くことにより、把持物体の目標力Ｆo dに対応する各アームの目標接触力Ｆi dを求めることができる。しかし、複数のアームによって把持物体を把持する場合には、把持行列は正方行列とならないので、一般には無限個の解が存在し、その一般解は次式で与えられる。
【数１６】
ここで、ｆint dは物体内部に生じさせる内力の目標値、Ｗ+はＷの疑似逆行列で、
【数１７】
で与えられる。また、Ｉは単位行列である。このようにして得られたＦi dをアームｉの手先力の目標値とする。
【００１７】
＜アームの制御則＞
力と速度の双対性により、ｉ番目のアームの手先速度（ｄｒi／ｄｔ）と、把持物体の速度（ｄｒo／ｄｔ）との間には次式が成立する。
【数１８】
これより、ｉ番目のアームの目標軌道では把持物体の目標軌道を用いて、次式のように与えられる。
【数１９】
このとき、アームの速度誤差は把持物体の速度誤差を使って次式のように書き表される。
【数２０】
また、参照速度を次式によって定義する。
【数２１】
ここでρ＞０である。また、剰余誤差ｓiを
【数２２】
として定義する。また、ｉ番目のアームの参照モデルを以下のように定義する。
【数２３】
ここでσiはｉ番目のアーム動力学パラメータベクトルである。さらに、アームｉの制御則とパラメータ適応則をそれぞれ、以下のようにする。
【数２４】
また、アームｉの関節トルクτiは
【数２５】
とする。ここでＪiはアームｉのヤコビ行列である。（２５）式で与えられる制御入力ｕiの右辺は、第１項目が参照モデルによるフィードフォーワード補償項、第２項目が手先力に対するフィードフォーワード補償項、そして第３項目がフィードバック補償項である。制御入力ｕiには力のフィードバックが含まれないので、力制御のために力センサを必要としない。
【００１８】
図２，図３，図４は、上述の把持物体（対象物体）とアーム（ロボット）の動力学のパラメータが未知の場合の、図１のような複数のロボットによる協調制御システムの構成を示す図である。図２は、図１に示すようなｋ台のロボットを用いて対象物体を操作している場合の概略構成例であり、１００はロボット目標軌道生成器で、２００はｋ台のロボット制御器からなるロボット制御器群、３００は実際に作業を遂行する例えば図１に示すアーム等のｋ台のロボットからなるロボット群で、４００はｋ台のロボット群の作業の対象となる対象物体（把持物体）である。
【００１９】
図２に表れる記号は、以下のような意味を有している。
【数２６】
ロボット目標軌道生成器１００は、タスク座標系で記述された有界な対象物体の目標位置軌道（ｒo d），目標速度軌道（ｄｒo d／ｄｔ）および内力の目標値ｆint dと観測された対象物体の位置（ｒo）と速度（ｄｒo／ｄｔ）を入力として、各ロボットの目標手先力（Ｆi d：ｉ＝１，２，．‥，ｋ）と目標手先軌道である位置軌道（ｒi d：ｉ＝１，２，…，ｋ）と速度軌道（ｄｒi d／ｄｔ：ｉ＝１，２，…，ｋ）を生成する。このロボット目標軌道生成器１００の構成は、後で図３を用いて詳しく説明する。
【００２０】
ロボット制御器群２００はｋ台のロボットに対応するｋ台のロボット制御器２１０より構成される。ｉ番目のロボット制御器はロボット目標軌道生成器１００によって生成された目標手先力（Ｆi d）、目標手先軌道（ｒi d，ｄｒi d／ｄｔ）および観測されたｉ番目のロボットの手先位置（ｒi）と手先速度（ｄｒi／ｄｔ）入力として、ｉ番目のロボットの制御入力（τi）を生成する。このロボット制御器に関して、後で図４を用いて詳しく説明する。
【００２１】
ロボット群３００は、例えば図１に示すようなｋ台の回転関節のみからなるロボットである。ｉ番目のロボットは、ロボット制御器群２００のｉ番目のロボット制御器によって生成された関節トルク（τi）が入力されることで、ロボットの手先に運動（ｒi，ｄｒi／ｄｔ）が生じ、同時に対象物体４００に接触力（Ｆi）をおよぼす。タスク座標系におけるｉ番目のロボットの手先位置は、ｉ番目のロボットの手先に設定する手先座標系の原点の位置とし、手先の姿勢は、例えばＺ−Ｙ−Ｚオイラー角を用いて記述する。
【００２２】
対象物体４００は任意の形状を持つ物体で、各ロボットより与えられる接触力（Ｆi：ｉ＝１，２，…，ｋ）により運動（ｒo，ｄｒo／ｄｔ）を生じる。タスク座標系における対象物体４００の位置は、対象物体４００に物体座標系を設定し、タスク座標系で観測した物体座標系原点の位置を用いて記述する。対象物体４００の姿勢は、例えばＺ−Ｙ−Ｚオイラー角を用いて記述することができる。
【００２３】
このような制御システムの構成により、対象物体の目標軌道と実際の軌道の観測値、及びロボットに把持されたときに生ずる内力の目標値から、各ロボットの目標軌道と目標手先力が生成される。各ロボットは、対応のロボット制御器により個別に与えられる目標軌道と目標手先力に追従する制御により制御される。これにより、全体としての協調制御を実現している。この様に構成したため、各ロボット制御器の間では制御のための相互通信を必要としておらず、独立に制御が行える。
【００２４】
＜対象物体の動力学パラメータが未知の場合のロボット目標軌道生成器＞
図３は、対象物体の動力学パラメータが未知のときの、ロボット目標軌道生成器１００の構成である。１１０は外力成分生成器で、１２０は内力成分生成器、１３０はロボット目標運動生成器、１１１は関数であるＹoを計算する計算部、１１２は微分フィルター、１１３は対象物体動力学パラメータ推定器、１２０は内力成分生成器、１３１はロボット目標位置軌道生成器、１３２はロボット目標速度軌道生成器である。
図３で用いられている記号を以下のように定義する。
【数２７】
対象物体の動力学モデルは、上記式（９）に示すように、動力学パラメータとその係数行列の積で表すことができる。Ｙo計算部１１１で、対象物体の動力学パラメータの係数行列（以降、リグレッサと呼ぶ）を計算することで、対象物体の動力学モデルを推定することができる。ρはゲイン定数で正のスカラである。Ｗは式（１５）で定義した把持行列で、各ロボットの把持点位置より計算される。
【００２５】
外力成分生成器１１０は、まず、対象物体目標位置ｒo dと対象物体位置ｒoとの差をとり対象物体位置誤差ｅoを求める。次に、式（１０）により、対象物体位置誤差ｅoにゲイン定数ｐを掛けたものと、対象物体目標速度（ｄｒor／ｄｔ）との和をとることで、対象物体参照速度を求める。次に、式（１３）により、対象物体速度と対象物体参照速度との差をとり対象物体剰余誤差ｓoを求める。一方、微分フィルター１１２は対象物体参照速度（ｄｒor／ｄｔ）より対象物体参照加速度（ｄ2ｒor／ｄｔ2）を計算する。Ｙo計算部１１１は、対象物体参照速度と参照加速度および対象物体位置と速度を用いて対象物体のリグレッサＹoを計算する（計算に関しては、例えば、発明者らの「拘束を受けるロボットの力・位置ハイブリッド適用制御」日本機械学会第２回ロボティクスシンポジア講演予稿集（１９９７年８月）等参照）。対象物体動力学パラメータ推定器１１３は計算された対象物体のリグレッサＹoと対象物体剰余誤差ｓoより対象物体の動力学パラメータの推定値を、
【数２８】
により計算する（式（１２）参照）。ここで、Γoは対象物体の適応ゲイン行列である。次に、動力学パラメータ推定値σoと対象物体リグレッサＹoとの積をとり対象物体の目標外力Ｆdを計算する。最後に、把持行列Ｗの擬似逆行列Ｗ+との積を計算し、ロボットの目標手先力の外力成分を求める。
内力成分生成器１２０は、内力の目標値ｆint dと（Ｉ−Ｗ+Ｗ）との積を計算してロボットの目標手先力の内力成分を計算する。
【００２６】
目標運動生成器１３０は、ロボット目標位置軌道生成器１３１とロボット目標速度軌道生成器１３２から構成される。目標位置軌道生成器１３１は、対象物体の目標位置軌道ｒo dより、式（１９）に従って、ロボットの手先の目標位置軌道ｒdを計算する。また、目標速度軌道生成器１３２は、式（２０）に従って、対象物体の目標速度軌道に把持行列の転置行列ＷTを掛けることにより、ロボットの手先の目標速度軌道を計算する。
【００２７】
目標軌道生成器１００は、最終的に式（１７）により、外力成分生成器１１０で計算された外力成分と、内力成分生成器１２０で生成された内力成分の和として、ロボットの目標手先力Ｆdを求める。さらに、ロボットの目標手先力Ｆdと、目標運動生成器１３０で計算されたロボットの目標手先位置軌道ｒdおよび目標速度軌道（ｄｒd／ｄｔ）とを各ロボット制御器に分配する。
【００２８】
以上の構成により、対象物体の参照軌道と目標内力を実現する各ロボットの目標手先力と目標軌道が生成される。対象物体の動力学パラメータが未知であっても、対象物体の動力学パラメータを推定しているので、各ロボットの目標手先力と目標軌道の生成が可能となっている。
【００２９】
＜各ロボットの動力学パラメータが未知の場合のロボット制御器＞
図４は、各ロボットの動力学パラメータが未知である場合のロボット制御器群２００におけるｉ番目のロボット制御器の構成例である。２１０はＹi計算部で、２２０は微分フィルター、２３０は動力学パラメータ推定器である。図４で用いられている記号の意味を以下に示す。
【数２９】
さて、ロボットの動力学モデルは、動力学パラメータとその係数行列（以下、ロボットのリグレッサと呼ぶ）の積で表すことができる。Ｙiはｉ番目のロボットのリグレッサである。Ｋiはｉ番目のロボットのフィードバック・ゲインで正定行列であり、Ｊiはｉ番目のロボットのヤコビ行列である。また、ρiはゲイン定数で正のスカラである。
【００３０】
図４において、ｉ番目のロボット制御器では、まず、ｉ番目のロボットの目標手先位置ｒi dと手先位置ｒiとの差をとり、ｉ番目のロボットの手先位置誤差ｅiを求める。次に、式（２２）に従って、ｉ番目のロボットの手先位置誤差ｅiにゲイン定数ρiを掛けたものとｉ番目のロボットの手先目標速度との和をとり、ｉ番目のロボットの手先参照速度を求める。式（２３）により、ｉ番目のロボットの手先速度と手先参照速度との差をとり、ｉ番目のロボットの手先剰余誤差ｓiを求める。一方、微分フィルター２２０は、ｉ番目のロボットの手先参照速度よりｉ番目のロボットの手先参照加速度を計算する。Ｙi計算部２１０は、ｉ番目のロボットの手先参照速度と参照加速度、および、ｉ番目のロボットの手先位置と速度により、ｉ番目のロボットのリグレッサＹiを計算する。動力学パラメータ推定器２３０は、計算されたロボットのリグレッサＹiと手先剰余誤差ｓiよりｉ番目のロボットの動力学パラメータの推定値を
【数３０】
により計算する（式（２６）参照）。ここで、Γiはロボット動力学パラメータの適応ゲイン行列である。
【００３１】
ｉ番目のロボット制御器は、計算された動力学パラメータ推定値σiとｉ番目のロボットのリグレッサＹiとの積をとり、ｉ番目のロボットの規範モデルによるフィードフォーワード項を計算し、ｉ番目のロボットの手先剰余誤差ｓiにゲイン行列Ｋiを掛けてフィードバック項を計算するとともに、ｉ番目のロボットの目標手先力Ｆi dを手先力のフィードフォーワード項とする。
ｉ番目のロボット制御器は、式（２５）に従って、規範モデルによるフィードフォーワード項、手先力のフィードフォーワード項およびフィードバック項を加え合わせることでｉ番目のロボットの制御入力ｕiを計算する。また、式（２７）に従って、ロボットのヤコビ行列Ｊiの転置行列Ｊi Tをｕiにかけてｉ番目のロボットの関節トルクに変換する。
【００３２】
以上の構成により、ロボットの動力学パラメータが未知であっても、ロボット目標軌道生成器から与えられる目標軌道と目標手先力に安定に追従させるロボット制御器となっている。各ロボット制御器は、他のロボット制御器との通信を必要としていない。また、力センサも必要としていない。
上記の図２、図３、図４で構成されるロボットの協調制御は、対象物体と各ロボットの動力学バラメータが未知であっても、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００３３】
＜各ロボットの動力学パラメータが既知＞
アームのパラメータが既知である場合には、アームの参照モデル（２４）式において、アームの動力学パラメータを推定する必要がなくなる。したがって、この場合、アームの制御則（２５）式は以下のように書き換えることができる。
【数３１】
これで分かるように、対象物体の適応則はそのまま用いることができる。
【００３４】
図５は、各ロボットの動力学パラメータが既知である場合のロボット制御器群２００のｉ番目のロボット制御器構成である。２４０はリグレッサＹi計算部で、２５０は微分フィルターである。図５における記号の意味は、図４と同様である。図４と比較すると、ロボットｉ動力学パラメータ推定器２３０が必要がなくなったことが分かる。
図５において、ｉ番目のロボット制御器は、ｉ番目のロボットの目標手先位置ｒi dと手先位置ｒiとの差をとりｉ番目のロボットの手先位置誤差ｅiを求める。次に、ｉ番目のロボットの手先位置誤差ｅiにゲイン定数ρiを掛けたものと、ｉ番目のロボットの手先目標速度との和をとり、手先参照速度を求める。次に、ｉ番目のロボットの手先速度と手先参照速度との差をとりｉ番目のロボットの手先剰余誤差ｓiを求める。一方、微分フィルター２５０はｉ番目のロボットの手先参照速度よりｉ番目のロボットの手先参照加速度を計算する。Ｙi計算部２４０はｉ番目のロボットの手先参照速度と参照加速度および手先位置と速度を用いてｉ番目のロボットのリグレッサＹiを計算する。
【００３５】
ｉ番目のロボット制御器は、既知であるｉ番目のロボットの動力学パラメータσiとｉ番目のロボットのリグレッサＹiとの積をとり、ｉ番目のロボットの規範モデルによるフィードフォーワード項を計算し、ｉ番目のロボットの手先剰余誤差ｓiにゲイン行列Ｋiを掛けてフィードバック項を計算するとともに、ｉ番目のロボットの目標手先力Ｆi dを手先力のフィードフォーワード項とする。
図５に示したｉ番目のロボット制御器は、上述の説明で分かるように、式（２８）に従って、規範モデルによるフィードフォーワード項、手先力のフィードフォーワード項およびフィードバック項を加え合わせることでｉ番目のロボットの制御入力ｕiを計算し、ロボットのヤコビ行列Ｊiの転置行列Ｊi Tをｕiにかけてｉ番目のロボットの関節トルクを計算する。
【００３６】
以上の構成により、ロボットの動力学パラメータが既知のとき、ロボット目標軌道生成器から与えられる目標軌道と目標手先力に安定に追従させるロボット制御器となっている。各ロボット制御器は、他のロボット制御器との通信を必要としていない。また、力センサも必要としていない。
【００３７】
上記の図２、図３、図５で構成されるロボットの協調制御は、対象物体の動力学パラメータが未知で各ロボットの動力学パラメータが既知のとき、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００３８】
＜把持物体とアームのパラメータがともに既知である場合＞
把持物体のパラメータも既知である場合には、把持物体の参照モデル（９）式において、把持物体の動力学パラメータさえも堆定する必要がなくなる。したがって、この場合、把持物体の適応則（１１）式は以下のように書き換えることができる。
【数３２】
また、アームの適応制御則は、上述で導かれたものをそのまま用いることができる。
図６は、対象物体の動力学パラメータが既知である場合のロボット目標軌道生成器１００の構成を示す。１４０は外力成分生成器で、１５０は内力成分生成器、１３０は目標運動生成器、１４１はＹo計算部、１４２は微分フィルターである。記号の意味は、図２と同様である。ρはゲイン定数で正のスカラである。Ｗは把持行列で、各ロボットの把持点位置より計算される。
【００３９】
外力成分生成器１４０は、対象物体目標位置ｒo dと対象物体位置ｒoとの差をとり対象物体位置誤差ｅoを求める。次に、対象物体位置誤差ｅoにゲイン定数ρを掛けたものと対象物体目標速度との和をとり対象物体参照速度を求める。次に、対象物体速度と対象物体参照速度との差をとり対象物体剰余誤差ｓoを求める。一方、微分フィルター１４２は対象物体参照速度より対象物体参照加速度を計算する。Ｙo計算部１４１は、対象物体の参照速度と参照加速度、対象物体の位置と速度、及び既知のロボットの動力学パラメータσoを用いて、対象物体リグレッサと動力学パラメータとの積Ｙoσoを計算する。
内力成分生成器１５０は内力の目標値ｆint dと（Ｉ−Ｗ+Ｗ）との積を計算して各ロボットの目標手先力の内力成分を計算する。
【００４０】
目標運動生成器１３０は、目標位置軌道生成器１３１と目標速度軌道生成器から構成される。目標位置軌道生成器１３１は、対象物体の目標位置ｒo dよりロボットの手先の目標位置軌道ｒdを計算する。また、目標速度軌道生成器１３２は対象物体の目標速度軌道ｒo dに把持行列の転置行列ＷTを掛けてロボットの手先の目標速度軌道ｒdを計算する。
【００４１】
図６に示した目標軌道生成器は、最終的に、上述の説明のように式（２９）に従って、外力成分生成器１４０で計算した外力成分と内力成分生成器１５０で生成した内力成分の和をとり、ロボットの目標手先力Ｆdを求める。そして、ロボットの目標手先力Ｆdと、目標運動生成器１３０で計算したロボットの目標手先位置軌道および目標速度軌道とを各ロボット制御器に分配する。
【００４２】
以上の構成により、対象物体の動力学パラメータが既知のとき、対象物体の参照軌道と目標内力を実現する各ロボットの目標手先力と目標軌道が生成される。上記の図２、図５、図６で構成されるロボットの協調制御は、対象物体と各ロボットの動力学パラメータが既知のとき、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００４３】
【発明の効果】
このような構成により、対象物体を複数のロボットで把持し作業を行なう制御装置において、対象物体の目標軌道を実現する目標力から各ロボットの運動と力の目標軌道が生成され、ロボットに目標運動を生じさせるフィードフォーワード補償と軌道誤差のフィードバック補償により、ロボットは運動の目標軌道に漸近的に収束し、その結果、対象物体の軌道も目標軌道に漸近的に収束する。対象物体の動力学パラメータが未知のときは、対象物体動力学パラメータ推定器により得る推定値を用いて、目標軌道が生成され、ロボットの動力学パラメータが未知のときは、ロボット動力学パラメータ推定器により得る推定値を用いて、フィードフォーワード補償項が生成され、いずれの実施例も、対象物体の軌道を目標軌道に漸近的、かつ安定に収束させることがLiapunov-Like Lemmaによって証明される。これらの実施例では、ロボットに高価でかつ脆弱な力センサが不要であるため、ロボット制御システムの低価格化と高信頼度化に役立つ。また、協調制御は、通常各ロボットの干渉を考慮して大掛かりな制御システムとなるが、この実施例での各ロボットの制御はロボット単体の制御として取り扱われ、ロボットの目標軌道生成器のみが追加されるため、制御システムの大規模化を避けることができる。
【００４４】
上述の説明では、ロボットの関節部に摩擦がある場合について述べていないが、動力学モデルに摩擦を含め、動力学パラメータに摩擦パラメータを加え、それに対応したリグレッサを構成することにより、同様な制御が構成できることは言うまでもない。また、各ロボットの目標軌道をタスク座標系で与えたが、このタスク座標系の目標軌道を等価な関節座標系の目標軌道として与えることにより、同様なロボット制御器が構成できることは言うまでもない。
【図面の簡単な説明】
【図１】複数のロボットによる物体の把持を示す図である。
【図２】実施形態の制御装置の構成を示す図である。
【図３】ロボット目標軌道生成のための装置構成例を示す図である。
【図４】ロボット制御のための装置構成例を示す図である。
【図５】ロボット制御のための他の構成例を示す図である。
【図６】ロボット目標軌道生成のための他の装置構成例を示す図である。
【発明の属する技術分野】
本発明は、対象物体を把持し作業を行う複数のロボットに対する協調制御に関するものである。
【０００２】
【従来の技術】
従来の、複数のロボットにより対象物体を把持して作業を行う協調制御方法として、各ロボットに力センサを装備し、力誤差と位置誤差をフィードバックして手先に任意のインピーダンス特性を保有させる方法が知られている（例えば、小菅一弘、吉田英博、他、”インピーダンス制御に基づく双腕マニピュレータの協調制御、”日本ロボット学会誌、Ｖｏｌ．１３、Ｎｏ．３、ＰＰ．４０４−１４０、１９９５参照）。この方法では、高価な力センサを必要とするため、制御系を構成するためのコストが高く、また、幾何学的誤差がある場合には位置と内力を正確に制御できないことが広く知られている。
【０００３】
また、複数のロボットにより対象物体を把持し作業を行う協調制御方法として、各ロボットに力センサを装着し、力誤差と運動誤差をフィードバックする方法が知られている（例えば、P.R.Pagilla and M.Tomizuka,“Adaptive Control of Two Robot Arms Carrying an Unknown Object,”Proc. IEEE Robotics and Automation,ＰＰ．５９７−６０２，１９９５ 参照）。この方法でも高価でかつ雑音の影響を受けやすい力センサを必要とするため、制御系を構成するためのコストが高く、制御システムの高信頼度化の妨げの要因となっていた。また、各ロボット毎に制御入力の生成ができないため、制御器を実装するコストを引き上げる要因となる。
【０００４】
さらに、二つのロボットのうち、一方のロボットが対象物体を把持し、他方のロボットが把持された対象物体に作業を行う協調制御方式も知られている（例えば、Y. H.Liu,S. Arimoto and K. Kitagaki,“Adaptive Control for holonomically constrained robots:time-invariant and time-variant cases,”Proc. IEEE Robotics and Automation,ＰＰ．９０５−９１２，１９９５ 参照）が、各ロボットとも力センサの装着を必要とするため、制御系のコストが高く、制御システムの高信頼度化を妨げる要因となっていた。
【０００５】
【発明が解決しようとする課題】
本発明の目的は、複数のロボットにより対象物体を把持し作業を行う協調制御において、各ロボットに力センサを必要としない、目標軌道に漸近安定な、並列処理可能な協調制御を提供することを目的としている。
【０００６】
【課題を解決するための手段】
上述の目的を達成するために、複数のロボットが、協調して対象物体を把持し作業を行う協調制御方法において、前記対象物体の動力学パラメータを推定し、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、前記推定された対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、各ロボットは、与えられた前記力と運動の目標軌道から動きを制御することを特徴とする。
上記制御方法は、ロボット目標軌道生成部と、前記複数の各ロボットに対応したロボット制御部とで構成され、協調して対象物体を把持し作業を行う協調制御装置に実装することができる。
各ロボットの動きに対する前記制御は、制御対象のロボットの動力学パラメータを推定し、該ロボットの推定した前記動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道とに基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより行うことができる。
【０００７】
上述の制御を用いることにより、各ロボットの手先に力センサを装備する必要はなく、対象物体を目標とする軌道に漸近的かつ安定に追従させることが可能である。
対象物体に関する動力学パラメータ推定、および各ロボットの動力学パラメータ推定と制御入力の計算は、すべて独立に行うことができ、複数の制御器や計算機に計算負荷を分配させることができる。これにより、複数のロボットによる協調制御が、高価な制御システムを構成することなく実現が可能となる。このとき、各ロボットは完全に独立して制御でき、各ロボット制御装置間の通信は必要とはしない。
さらに、対象物体の目標軌道は作業座標系で記述されるため、ロボットの関節変位で対象物体の目標軌道を記述する必要がなく、実際の現揚で目標軌道を生成することが容易である。
【０００８】
また、ロボットの動力学パラメータが既知の場合は、各ロボットの動きに対する前記制御は、該ロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより行うこともできる。このように、各ロボットの動力学パラメータが既知のとき、各ロボットの動力学のパラメータ推定を省略し、既知の値を用いてフィードフォーワード補償項を構成することができる。この場合、対象物体の位置姿勢のみでなく、対象物体の内部に生ずる内力を、力センサを用いなくとも目標内力に収束させることができる。
【０００９】
対象物体の動力学パラメータとロボットの動力学パラメータが既知の場合は、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、各ロボットは、該ロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより、与えられた前記力と運動の目標軌道から動きを制御することもできる。
【００１０】
このように、対象物体やロボットの動力学パラメータが既知のとき、ロボットおよび対象物体の動力学パラメータの推定を省略し、既知の値を用いてフィードフォーワード補償項を構成することができる。この場合も、対象物体の位置姿勢とともに、対象物体内部に生ずる内力を、力センサを用いずに、目標内力に漸近的に収束させることができる。
【００１１】
【発明の実施の形態】
本発明の実施形態を、図面を参照して詳細に説明する。
図１に示すような複数のロボット（アームまたは指）による物体把持を考える。図１で用いる記号を以下に示す。
【数１】
（１）物体を把持するアームはｋ本である。
（２）アームの手先と物体表面の接触は摩擦のある接触、または、ハンドにより堅く把持され、手先に任意の力とモーメントを生じさせることができる。
（３）物体座標系から観測して、接触点は運動をしない。
（４）アームの手先は接触点から離れることはない、すなわち、Ｐi＝Ｐci
（５）ｐociは計測できる。
【００１２】
＜把持物体の運動方程式＞
把持物体（対象物体）は、一般に次の運動方程式によって記述される。
【数２】
【数３】
【００１３】
＜把持行列＞
外力ベクトルＦoを、
【数４】
【数５】
【数６】
【数７】
【００１４】
＜アームｉの運動方程式＞
タスク空間で記述したアームｉの運動方程式が、
【数８】
【００１５】
本発明における適応協調制御は、タスク座標系で与えられた把持物体の目標軌道と、物体内部に発生させる目標内力ｆint dに対して、
（１）把持物体に目標となる運動を生じさせ、また目標内力を生じさせるための、把持物体に加えるべき目標外力を把持物体の参照モデルを使って計算し、
（２）得られた把持物体の目標外力から各アームの手先の力目標値と目標軌道を計算し、それらを各アームのコントローラに分配し、
（３）アームのコントローラによって、把持物体に目標とする運動を生じさせるように、手先の運動と手先力を適応制御を使って各アームを制御する。
というものである。この適応制御の特徴は、力制御は力のフィードフォーワードのみで、力フィードバックを用いないため、力の計測が不要であること、および把持物体とロボットの動力学パラメータが未知であっても、把持物体を目標軌道に漸近的、かつ安定に追従させることができることにある。以下に詳しく述べる。
【００１６】
＜把持物体に対する適応則＞
把持物体の参照モデルを以下のように定義する。
【数９】
【数１０】
【数１１】
【数１２】
＜内力の目標値とアームの目標手先力＞
各アームが発生する手先力Ｆiからなる手先力ベクトルを、
【数１３】
【数１４】
【数１５】
【数１６】
【数１７】
【００１７】
＜アームの制御則＞
力と速度の双対性により、ｉ番目のアームの手先速度（ｄｒi／ｄｔ）と、把持物体の速度（ｄｒo／ｄｔ）との間には次式が成立する。
【数１８】
【数１９】
【数２０】
【数２１】
【数２２】
【数２３】
【数２４】
【数２５】
【００１８】
図２，図３，図４は、上述の把持物体（対象物体）とアーム（ロボット）の動力学のパラメータが未知の場合の、図１のような複数のロボットによる協調制御システムの構成を示す図である。図２は、図１に示すようなｋ台のロボットを用いて対象物体を操作している場合の概略構成例であり、１００はロボット目標軌道生成器で、２００はｋ台のロボット制御器からなるロボット制御器群、３００は実際に作業を遂行する例えば図１に示すアーム等のｋ台のロボットからなるロボット群で、４００はｋ台のロボット群の作業の対象となる対象物体（把持物体）である。
【００１９】
図２に表れる記号は、以下のような意味を有している。
【数２６】
【００２０】
ロボット制御器群２００はｋ台のロボットに対応するｋ台のロボット制御器２１０より構成される。ｉ番目のロボット制御器はロボット目標軌道生成器１００によって生成された目標手先力（Ｆi d）、目標手先軌道（ｒi d，ｄｒi d／ｄｔ）および観測されたｉ番目のロボットの手先位置（ｒi）と手先速度（ｄｒi／ｄｔ）入力として、ｉ番目のロボットの制御入力（τi）を生成する。このロボット制御器に関して、後で図４を用いて詳しく説明する。
【００２１】
ロボット群３００は、例えば図１に示すようなｋ台の回転関節のみからなるロボットである。ｉ番目のロボットは、ロボット制御器群２００のｉ番目のロボット制御器によって生成された関節トルク（τi）が入力されることで、ロボットの手先に運動（ｒi，ｄｒi／ｄｔ）が生じ、同時に対象物体４００に接触力（Ｆi）をおよぼす。タスク座標系におけるｉ番目のロボットの手先位置は、ｉ番目のロボットの手先に設定する手先座標系の原点の位置とし、手先の姿勢は、例えばＺ−Ｙ−Ｚオイラー角を用いて記述する。
【００２２】
対象物体４００は任意の形状を持つ物体で、各ロボットより与えられる接触力（Ｆi：ｉ＝１，２，…，ｋ）により運動（ｒo，ｄｒo／ｄｔ）を生じる。タスク座標系における対象物体４００の位置は、対象物体４００に物体座標系を設定し、タスク座標系で観測した物体座標系原点の位置を用いて記述する。対象物体４００の姿勢は、例えばＺ−Ｙ−Ｚオイラー角を用いて記述することができる。
【００２３】
このような制御システムの構成により、対象物体の目標軌道と実際の軌道の観測値、及びロボットに把持されたときに生ずる内力の目標値から、各ロボットの目標軌道と目標手先力が生成される。各ロボットは、対応のロボット制御器により個別に与えられる目標軌道と目標手先力に追従する制御により制御される。これにより、全体としての協調制御を実現している。この様に構成したため、各ロボット制御器の間では制御のための相互通信を必要としておらず、独立に制御が行える。
【００２４】
＜対象物体の動力学パラメータが未知の場合のロボット目標軌道生成器＞
図３は、対象物体の動力学パラメータが未知のときの、ロボット目標軌道生成器１００の構成である。１１０は外力成分生成器で、１２０は内力成分生成器、１３０はロボット目標運動生成器、１１１は関数であるＹoを計算する計算部、１１２は微分フィルター、１１３は対象物体動力学パラメータ推定器、１２０は内力成分生成器、１３１はロボット目標位置軌道生成器、１３２はロボット目標速度軌道生成器である。
図３で用いられている記号を以下のように定義する。
【数２７】
【００２５】
外力成分生成器１１０は、まず、対象物体目標位置ｒo dと対象物体位置ｒoとの差をとり対象物体位置誤差ｅoを求める。次に、式（１０）により、対象物体位置誤差ｅoにゲイン定数ｐを掛けたものと、対象物体目標速度（ｄｒor／ｄｔ）との和をとることで、対象物体参照速度を求める。次に、式（１３）により、対象物体速度と対象物体参照速度との差をとり対象物体剰余誤差ｓoを求める。一方、微分フィルター１１２は対象物体参照速度（ｄｒor／ｄｔ）より対象物体参照加速度（ｄ2ｒor／ｄｔ2）を計算する。Ｙo計算部１１１は、対象物体参照速度と参照加速度および対象物体位置と速度を用いて対象物体のリグレッサＹoを計算する（計算に関しては、例えば、発明者らの「拘束を受けるロボットの力・位置ハイブリッド適用制御」日本機械学会第２回ロボティクスシンポジア講演予稿集（１９９７年８月）等参照）。対象物体動力学パラメータ推定器１１３は計算された対象物体のリグレッサＹoと対象物体剰余誤差ｓoより対象物体の動力学パラメータの推定値を、
【数２８】
内力成分生成器１２０は、内力の目標値ｆint dと（Ｉ−Ｗ+Ｗ）との積を計算してロボットの目標手先力の内力成分を計算する。
【００２６】
目標運動生成器１３０は、ロボット目標位置軌道生成器１３１とロボット目標速度軌道生成器１３２から構成される。目標位置軌道生成器１３１は、対象物体の目標位置軌道ｒo dより、式（１９）に従って、ロボットの手先の目標位置軌道ｒdを計算する。また、目標速度軌道生成器１３２は、式（２０）に従って、対象物体の目標速度軌道に把持行列の転置行列ＷTを掛けることにより、ロボットの手先の目標速度軌道を計算する。
【００２７】
目標軌道生成器１００は、最終的に式（１７）により、外力成分生成器１１０で計算された外力成分と、内力成分生成器１２０で生成された内力成分の和として、ロボットの目標手先力Ｆdを求める。さらに、ロボットの目標手先力Ｆdと、目標運動生成器１３０で計算されたロボットの目標手先位置軌道ｒdおよび目標速度軌道（ｄｒd／ｄｔ）とを各ロボット制御器に分配する。
【００２８】
以上の構成により、対象物体の参照軌道と目標内力を実現する各ロボットの目標手先力と目標軌道が生成される。対象物体の動力学パラメータが未知であっても、対象物体の動力学パラメータを推定しているので、各ロボットの目標手先力と目標軌道の生成が可能となっている。
【００２９】
＜各ロボットの動力学パラメータが未知の場合のロボット制御器＞
図４は、各ロボットの動力学パラメータが未知である場合のロボット制御器群２００におけるｉ番目のロボット制御器の構成例である。２１０はＹi計算部で、２２０は微分フィルター、２３０は動力学パラメータ推定器である。図４で用いられている記号の意味を以下に示す。
【数２９】
【００３０】
図４において、ｉ番目のロボット制御器では、まず、ｉ番目のロボットの目標手先位置ｒi dと手先位置ｒiとの差をとり、ｉ番目のロボットの手先位置誤差ｅiを求める。次に、式（２２）に従って、ｉ番目のロボットの手先位置誤差ｅiにゲイン定数ρiを掛けたものとｉ番目のロボットの手先目標速度との和をとり、ｉ番目のロボットの手先参照速度を求める。式（２３）により、ｉ番目のロボットの手先速度と手先参照速度との差をとり、ｉ番目のロボットの手先剰余誤差ｓiを求める。一方、微分フィルター２２０は、ｉ番目のロボットの手先参照速度よりｉ番目のロボットの手先参照加速度を計算する。Ｙi計算部２１０は、ｉ番目のロボットの手先参照速度と参照加速度、および、ｉ番目のロボットの手先位置と速度により、ｉ番目のロボットのリグレッサＹiを計算する。動力学パラメータ推定器２３０は、計算されたロボットのリグレッサＹiと手先剰余誤差ｓiよりｉ番目のロボットの動力学パラメータの推定値を
【数３０】
【００３１】
ｉ番目のロボット制御器は、計算された動力学パラメータ推定値σiとｉ番目のロボットのリグレッサＹiとの積をとり、ｉ番目のロボットの規範モデルによるフィードフォーワード項を計算し、ｉ番目のロボットの手先剰余誤差ｓiにゲイン行列Ｋiを掛けてフィードバック項を計算するとともに、ｉ番目のロボットの目標手先力Ｆi dを手先力のフィードフォーワード項とする。
ｉ番目のロボット制御器は、式（２５）に従って、規範モデルによるフィードフォーワード項、手先力のフィードフォーワード項およびフィードバック項を加え合わせることでｉ番目のロボットの制御入力ｕiを計算する。また、式（２７）に従って、ロボットのヤコビ行列Ｊiの転置行列Ｊi Tをｕiにかけてｉ番目のロボットの関節トルクに変換する。
【００３２】
以上の構成により、ロボットの動力学パラメータが未知であっても、ロボット目標軌道生成器から与えられる目標軌道と目標手先力に安定に追従させるロボット制御器となっている。各ロボット制御器は、他のロボット制御器との通信を必要としていない。また、力センサも必要としていない。
上記の図２、図３、図４で構成されるロボットの協調制御は、対象物体と各ロボットの動力学バラメータが未知であっても、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００３３】
＜各ロボットの動力学パラメータが既知＞
アームのパラメータが既知である場合には、アームの参照モデル（２４）式において、アームの動力学パラメータを推定する必要がなくなる。したがって、この場合、アームの制御則（２５）式は以下のように書き換えることができる。
【数３１】
【００３４】
図５は、各ロボットの動力学パラメータが既知である場合のロボット制御器群２００のｉ番目のロボット制御器構成である。２４０はリグレッサＹi計算部で、２５０は微分フィルターである。図５における記号の意味は、図４と同様である。図４と比較すると、ロボットｉ動力学パラメータ推定器２３０が必要がなくなったことが分かる。
図５において、ｉ番目のロボット制御器は、ｉ番目のロボットの目標手先位置ｒi dと手先位置ｒiとの差をとりｉ番目のロボットの手先位置誤差ｅiを求める。次に、ｉ番目のロボットの手先位置誤差ｅiにゲイン定数ρiを掛けたものと、ｉ番目のロボットの手先目標速度との和をとり、手先参照速度を求める。次に、ｉ番目のロボットの手先速度と手先参照速度との差をとりｉ番目のロボットの手先剰余誤差ｓiを求める。一方、微分フィルター２５０はｉ番目のロボットの手先参照速度よりｉ番目のロボットの手先参照加速度を計算する。Ｙi計算部２４０はｉ番目のロボットの手先参照速度と参照加速度および手先位置と速度を用いてｉ番目のロボットのリグレッサＹiを計算する。
【００３５】
ｉ番目のロボット制御器は、既知であるｉ番目のロボットの動力学パラメータσiとｉ番目のロボットのリグレッサＹiとの積をとり、ｉ番目のロボットの規範モデルによるフィードフォーワード項を計算し、ｉ番目のロボットの手先剰余誤差ｓiにゲイン行列Ｋiを掛けてフィードバック項を計算するとともに、ｉ番目のロボットの目標手先力Ｆi dを手先力のフィードフォーワード項とする。
図５に示したｉ番目のロボット制御器は、上述の説明で分かるように、式（２８）に従って、規範モデルによるフィードフォーワード項、手先力のフィードフォーワード項およびフィードバック項を加え合わせることでｉ番目のロボットの制御入力ｕiを計算し、ロボットのヤコビ行列Ｊiの転置行列Ｊi Tをｕiにかけてｉ番目のロボットの関節トルクを計算する。
【００３６】
以上の構成により、ロボットの動力学パラメータが既知のとき、ロボット目標軌道生成器から与えられる目標軌道と目標手先力に安定に追従させるロボット制御器となっている。各ロボット制御器は、他のロボット制御器との通信を必要としていない。また、力センサも必要としていない。
【００３７】
上記の図２、図３、図５で構成されるロボットの協調制御は、対象物体の動力学パラメータが未知で各ロボットの動力学パラメータが既知のとき、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００３８】
＜把持物体とアームのパラメータがともに既知である場合＞
把持物体のパラメータも既知である場合には、把持物体の参照モデル（９）式において、把持物体の動力学パラメータさえも堆定する必要がなくなる。したがって、この場合、把持物体の適応則（１１）式は以下のように書き換えることができる。
【数３２】
図６は、対象物体の動力学パラメータが既知である場合のロボット目標軌道生成器１００の構成を示す。１４０は外力成分生成器で、１５０は内力成分生成器、１３０は目標運動生成器、１４１はＹo計算部、１４２は微分フィルターである。記号の意味は、図２と同様である。ρはゲイン定数で正のスカラである。Ｗは把持行列で、各ロボットの把持点位置より計算される。
【００３９】
外力成分生成器１４０は、対象物体目標位置ｒo dと対象物体位置ｒoとの差をとり対象物体位置誤差ｅoを求める。次に、対象物体位置誤差ｅoにゲイン定数ρを掛けたものと対象物体目標速度との和をとり対象物体参照速度を求める。次に、対象物体速度と対象物体参照速度との差をとり対象物体剰余誤差ｓoを求める。一方、微分フィルター１４２は対象物体参照速度より対象物体参照加速度を計算する。Ｙo計算部１４１は、対象物体の参照速度と参照加速度、対象物体の位置と速度、及び既知のロボットの動力学パラメータσoを用いて、対象物体リグレッサと動力学パラメータとの積Ｙoσoを計算する。
内力成分生成器１５０は内力の目標値ｆint dと（Ｉ−Ｗ+Ｗ）との積を計算して各ロボットの目標手先力の内力成分を計算する。
【００４０】
目標運動生成器１３０は、目標位置軌道生成器１３１と目標速度軌道生成器から構成される。目標位置軌道生成器１３１は、対象物体の目標位置ｒo dよりロボットの手先の目標位置軌道ｒdを計算する。また、目標速度軌道生成器１３２は対象物体の目標速度軌道ｒo dに把持行列の転置行列ＷTを掛けてロボットの手先の目標速度軌道ｒdを計算する。
【００４１】
図６に示した目標軌道生成器は、最終的に、上述の説明のように式（２９）に従って、外力成分生成器１４０で計算した外力成分と内力成分生成器１５０で生成した内力成分の和をとり、ロボットの目標手先力Ｆdを求める。そして、ロボットの目標手先力Ｆdと、目標運動生成器１３０で計算したロボットの目標手先位置軌道および目標速度軌道とを各ロボット制御器に分配する。
【００４２】
以上の構成により、対象物体の動力学パラメータが既知のとき、対象物体の参照軌道と目標内力を実現する各ロボットの目標手先力と目標軌道が生成される。上記の図２、図５、図６で構成されるロボットの協調制御は、対象物体と各ロボットの動力学パラメータが既知のとき、対象物体を目標軌道に漸近的に追従させることができることをLiapunov-Like Lemmaにより理論的に証明できる。
【００４３】
【発明の効果】
このような構成により、対象物体を複数のロボットで把持し作業を行なう制御装置において、対象物体の目標軌道を実現する目標力から各ロボットの運動と力の目標軌道が生成され、ロボットに目標運動を生じさせるフィードフォーワード補償と軌道誤差のフィードバック補償により、ロボットは運動の目標軌道に漸近的に収束し、その結果、対象物体の軌道も目標軌道に漸近的に収束する。対象物体の動力学パラメータが未知のときは、対象物体動力学パラメータ推定器により得る推定値を用いて、目標軌道が生成され、ロボットの動力学パラメータが未知のときは、ロボット動力学パラメータ推定器により得る推定値を用いて、フィードフォーワード補償項が生成され、いずれの実施例も、対象物体の軌道を目標軌道に漸近的、かつ安定に収束させることがLiapunov-Like Lemmaによって証明される。これらの実施例では、ロボットに高価でかつ脆弱な力センサが不要であるため、ロボット制御システムの低価格化と高信頼度化に役立つ。また、協調制御は、通常各ロボットの干渉を考慮して大掛かりな制御システムとなるが、この実施例での各ロボットの制御はロボット単体の制御として取り扱われ、ロボットの目標軌道生成器のみが追加されるため、制御システムの大規模化を避けることができる。
【００４４】
上述の説明では、ロボットの関節部に摩擦がある場合について述べていないが、動力学モデルに摩擦を含め、動力学パラメータに摩擦パラメータを加え、それに対応したリグレッサを構成することにより、同様な制御が構成できることは言うまでもない。また、各ロボットの目標軌道をタスク座標系で与えたが、このタスク座標系の目標軌道を等価な関節座標系の目標軌道として与えることにより、同様なロボット制御器が構成できることは言うまでもない。
【図面の簡単な説明】
【図１】複数のロボットによる物体の把持を示す図である。
【図２】実施形態の制御装置の構成を示す図である。
【図３】ロボット目標軌道生成のための装置構成例を示す図である。
【図４】ロボット制御のための装置構成例を示す図である。
【図５】ロボット制御のための他の構成例を示す図である。
【図６】ロボット目標軌道生成のための他の装置構成例を示す図である。
Claims (8)
- 複数のロボットが、協調して対象物体を把持し作業を行う協調制御方法において、
前記対象物体の動力学パラメータを推定し、
対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、前記推定された対象物体の動力学パラメータと動力学モデルとを使って求め、
求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、
各ロボットは、与えられた前記力と運動の目標軌道から動きを制御する
ことを特徴とする協調制御方法。 - 請求項１に記載のロボットの協調制御方法において、
各ロボットの動きに対する前記制御は、
制御対象のロボットの動力学パラメータを推定し、
該ロボットの推定した前記動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道とに基づくフィードフォーワード補償と、
該ロボットの運動の軌道誤差に基づくフィードバック補償と
により行うことを特徴とする協調制御方法。 - 請求項１に記載のロボットの協調制御方法において、
各ロボットの動きに対する前記制御は、
制御対象のロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、
該ロボットの運動の軌道誤差に基づくフィードバック補償と
により行うことを特徴とする協調制御方法。 - 複数のロボットが、協調して対象物体を把持し作業を行う協調制御方法において、
対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、対象物体の動力学パラメータと動力学モデルとを使って求め、
求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、
各ロボットは、
制御対象ロボットの動力学パラメータと
該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、
該ロボットの運動の軌道誤差に基づくフィードバック補償と
により、与えられた前記力と運動の目標軌道から動きを制御することを特徴とする協調制御方法。 - 複数のロボットが、協調して対象物体を把持し作業を行う協調制御装置において、
ロボット目標軌道生成部と、前記複数の各ロボットに対応したロボット制御部とで構成され、
前記ロボット目標軌道生成部は、前記対象物体の動力学パラメータを推定し、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、前記推定された対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、
前記ロボット制御部は、与えられた前記力と運動の目標軌道から、各ロボットの動きを制御する
ことを特徴とする協調制御装置。 - 請求項５に記載のロボットの協調制御装置において、
前記ロボット制御部は、
制御対象のロボットの動力学パラメータを推定し、
該ロボットの推定した前記動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道とに基づくフィードフォーワード補償と、該ロボットの運動の軌道誤差に基づくフィードバック補償とにより、
各ロボットの動きに対する制御を行うことを特徴とする協調制御装置。 - 請求項５に記載のロボットの協調制御装置において、
前記ロボット制御部は、
制御対象のロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、
該ロボットの運動の軌道誤差に基づくフィードバック補償とにより、
各ロボットの動きに対する制御を行うことを特徴とする協調制御装置。 - 複数のロボットが、協調して対象物体を把持し作業を行う協調制御装置において、
ロボット目標軌道生成部と、前記複数の各ロボットに対応したロボット制御部とで構成され、
前記ロボット目標軌道生成部は、対象物体に、タスク座標系で与えられた対象物体の目標軌道による運動を生じさせる対象物体に加えるべき目標の力を、対象物体の動力学パラメータと動力学モデルとを使って求め、求められた対象物体に加えるべき前記目標の力から各ロボットの力の目標軌道を求めるとともに、タスク座標系で与えられた対象物体の目標軌道から各ロボットの運動の目標軌道を求めて、これらの各ロボットに対する力および運動の目標軌道を各ロボットに分配し、
前記ロボット制御部は、
制御対象のロボットの動力学パラメータと該ロボットの動力学モデルおよび力の目標軌道に基づくフィードフォーワード補償と、
該ロボットの運動の軌道誤差に基づくフィードバック補償とにより、与えられた前記力と運動の目標軌道から動きを制御することを特徴とする協調制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000209129A JP3765713B2 (ja) | 2000-07-10 | 2000-07-10 | ロボットの協調制御方法及び協調制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000209129A JP3765713B2 (ja) | 2000-07-10 | 2000-07-10 | ロボットの協調制御方法及び協調制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2002018752A JP2002018752A (ja) | 2002-01-22 |
| JP3765713B2 true JP3765713B2 (ja) | 2006-04-12 |
Family
ID=18705586
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2000209129A Expired - Fee Related JP3765713B2 (ja) | 2000-07-10 | 2000-07-10 | ロボットの協調制御方法及び協調制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3765713B2 (ja) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101294348B1 (ko) | 2012-05-11 | 2013-08-07 | 재단법인대구경북과학기술원 | 원격제어 로봇의 자율적 협업 기반 밸브 조작 방법 및 그 시스템 |
| KR101321984B1 (ko) | 2012-05-08 | 2013-10-28 | 재단법인대구경북과학기술원 | 협업기반 밸브 조작을 위한 로봇의 경로보상 방법 및 그 시스템 |
| KR101383722B1 (ko) * | 2012-12-17 | 2014-04-08 | 현대자동차(주) | 로봇의 양팔제어방법 |
| CN110658811A (zh) * | 2019-09-09 | 2020-01-07 | 华南理工大学 | 基于神经网络的受限移动机器人协同路径跟踪控制方法 |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008087141A (ja) * | 2006-10-05 | 2008-04-17 | Toyota Motor Corp | 脚式移動ロボットの動作データ作成装置及び作成方法 |
| JP5614232B2 (ja) * | 2010-10-20 | 2014-10-29 | トヨタ自動車株式会社 | ロボットハンドの制御装置、制御方法、及び制御用プログラム |
| WO2014002678A1 (ja) | 2012-06-29 | 2014-01-03 | 三菱電機株式会社 | ロボット制御装置およびロボット制御方法 |
| CN103123488B (zh) * | 2013-01-18 | 2015-02-25 | 西北工业大学 | 空间绳系机器人系统逼近目标协调控制方法 |
| CN106003049B (zh) * | 2016-07-05 | 2018-06-12 | 李亚楠 | 人-机协作系统的控制方法 |
| CN108415425B (zh) * | 2018-02-08 | 2020-10-30 | 东华大学 | 一种基于改进基因调控网络的分布式群机器人协同集群算法 |
| CN110405758B (zh) * | 2019-07-15 | 2022-08-09 | 西北工业大学 | 基于非奇异终端滑模技术的空间机器人目标抓捕控制方法 |
| CN113967916B (zh) * | 2021-11-22 | 2023-01-10 | 浙江工业大学 | 一种基于集中式预测控制的多机器人协作搬运控制方法 |
| CN114721269B (zh) * | 2022-04-11 | 2023-04-28 | 江南大学 | 基于脉冲窗的受扰非线性多智能体准一致性方法及系统 |
| CN114706312B (zh) * | 2022-06-06 | 2022-09-30 | 中国科学技术大学 | 一种绳索牵引并联机器人高精度自适应协同控制方法 |
-
2000
- 2000-07-10 JP JP2000209129A patent/JP3765713B2/ja not_active Expired - Fee Related
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101321984B1 (ko) | 2012-05-08 | 2013-10-28 | 재단법인대구경북과학기술원 | 협업기반 밸브 조작을 위한 로봇의 경로보상 방법 및 그 시스템 |
| KR101294348B1 (ko) | 2012-05-11 | 2013-08-07 | 재단법인대구경북과학기술원 | 원격제어 로봇의 자율적 협업 기반 밸브 조작 방법 및 그 시스템 |
| KR101383722B1 (ko) * | 2012-12-17 | 2014-04-08 | 현대자동차(주) | 로봇의 양팔제어방법 |
| US9162356B2 (en) | 2012-12-17 | 2015-10-20 | Hyundai Motor Company | Method for controlling two arms of a robot |
| CN110658811A (zh) * | 2019-09-09 | 2020-01-07 | 华南理工大学 | 基于神经网络的受限移动机器人协同路径跟踪控制方法 |
| CN110658811B (zh) * | 2019-09-09 | 2020-09-18 | 华南理工大学 | 基于神经网络的受限移动机器人协同路径跟踪控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2002018752A (ja) | 2002-01-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3765713B2 (ja) | ロボットの協調制御方法及び協調制御装置 | |
| CN111319036B (zh) | 基于自适应算法的移动机械臂位置/力自抗扰控制方法 | |
| CN112180729B (zh) | 一种机械臂自适应轨迹跟踪控制方法 | |
| Li et al. | Adaptive robust motion/force control of holonomic-constrained nonholonomic mobile manipulators | |
| Li et al. | Neural-adaptive control of single-master–multiple-slaves teleoperation for coordinated multiple mobile manipulators with time-varying communication delays and input uncertainties | |
| JP5327722B2 (ja) | ロボットの負荷推定装置及び負荷推定方法 | |
| Ajwad et al. | Disturbance-observer-based robust control of a serial-link robotic manipulator using SMC and PBC techniques | |
| Kawasaki et al. | Decentralized adaptive coordinated control of multiple robot arms without using a force sensor | |
| Li et al. | Model-free impedance control for safe human-robot interaction | |
| CN112809666B (zh) | 一种基于神经网络的5-dof机械臂力位跟踪算法 | |
| CN115625711A (zh) | 一种考虑末端力的双臂机器人协同控制方法 | |
| Deylami et al. | Observer‐based adaptive control of cooperative multiple manipulators using the Mastroianni operators as uncertainty approximator | |
| Aghili | Self-tuning cooperative control of manipulators with position/orientation uncertainties in the closed-kinematic loop | |
| Hu et al. | Motion and force control of coordinated robots during constrained motion tasks | |
| Venkatesan et al. | Disturbance observer based terminal sliding mode control of an underwater manipulator | |
| He et al. | Dual impedance control with variable object stiffness for the dual-arm cooperative manipulators | |
| CN116901057A (zh) | 一种基于变阻抗策略的双机械臂末端位置协同控制方法 | |
| CN115890735A (zh) | 机械臂系统、机械臂及其控制方法、控制器和存储介质 | |
| Lei et al. | Dual-arm object transportation via model predictive control and external disturbance estimation | |
| Wu et al. | An adaptive neural network compensator for decoupling of dynamic effects of a macro-mini manipulator | |
| Smith et al. | Biomimetic joint/task space hybrid adaptive control for bimanual robotic manipulation | |
| Ueki et al. | Adaptive coordinated control of multi-fingered hands with rolling contact | |
| Hashlamon | Sliding mode based virtual sensor for feedback linearization in delta robots with unknown dynamic model | |
| CN113400316B (zh) | 建筑垃圾分拣机械手抓取控制方法及装置 | |
| CN108614430A (zh) | 一种用于非完全连接约束的空间组合体数据驱动控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20051006 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20051018 |
|
| RD02 | Notification of acceptance of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7422 Effective date: 20051021 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20051128 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20060117 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20060124 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |