1 (57)【特許請求の範囲】【請求項１】位置及び摩擦力を制御する多関節研磨ロボットであって、多関節研磨ロボットの関節座標系における運動方程式をヤコビアンを用いて関節座標系から作業座標系に変換した多関節研磨ロボットの作業座標系における運動方程式に基づき予め制御設計した制御対象と、作業座標系上の力の目標値と力の検出値間の誤差及び作業座標系上の位置の目標値と位置の検出値間の誤差に基づき多関節研磨ロボット特有の非線形性を補償するスライディングモード非干渉制御手段と、該スライディ 10 ングモード非干渉制御手段で得た制御入力信号に対し既知のパラメータ変動の逆ダイナミクスを加えた加算信号に基づき多関節研磨ロボットの各関節トルクを算出する関節トルク算出手段と、該関節トルク算出手段で算出した各関節トルクに基づき各関節を駆動する関節駆動手段 2 を備えたことを特徴とするスライディングモード非干渉制御を用いた多関節研磨ロボット。【発明の詳細な説明】【０００１】【発明の属する技術分野】本発明はスライディングモード非干渉制御を用いた多関節研磨ロボットに係わり、特に多関節研磨ロボットにスライディングモード非干渉制御を適用可能としたことで複雑曲面の高精度研磨を可能とし、かつチャタリングの発生を低減したスライディングモード非干渉制御を用いた多関節研磨ロボットに関する。【０００２】【従来の技術】従来、ロボット１に加工対象となるワーク３を把持させて回転型研磨装置５に押し当てる方式の研磨システムが知られている（図示略）。かかる研磨シ (2) 特許３２１５８４１ 3 4 ステムに対しては、ワーク３の位置と研磨面に作用する＊ド非干渉制御について説明する。まず、制御対象を数１接線方向力すなわち摩擦力をスライディングモード非干のように記述する。渉制御を用いて制御することが可能である（平成９年特【０００３】許願第１２４９３３号）。ここで、スライディングモー＊【数１】但し、Ａ（ｎ×ｎ），Ｂ（ｎ×ｍ），Ｃ（ｍ×ｎ），ｄ ※ 次にＣ行列と微分可能な目標値ｒ（ｔ）を用い係数行列（ｘ，ｔ）は大きさが既知な非線形外乱で数２が成り立数５を定義し、超平面σ（ｔ）を設計する。ち、ｄｅｔ（ＣＢ）≠０が成り立たずに数３が成り立【０００６】つ。 10 【数５】【０００４】【数２】このとき数６が成り立つ。【０００７】【数３】【数６】但し、ｃi はＣ行列のｉ行目を意味する。よく知られるように数４が正則ならば制御系の非干渉化が可能である。 20 【０００５】【数４】【数７】【数８】 ※ 【数９】このとき、制御入力として次の数１０、数１１を用い、 ★ 関数となり、σi 各行に対するリアプノフ関数の候補をＶi （ｔ）＝σi 40 【０００８】 2 （ｔ）／２≧０として数１２を求める。Ｋi （ｘ，【数１０】ｔ）を数１４のように選べば、Ｖi （ｔ）はリアプノフ★ 【数１１】 50 (k) →０，ｋ≧０を得る。 (3) 5 特許３２１５８４１ 6 2 ＊また，Ｖi （ｔ）＝σi （ｔ）／２≧０時の等価線形系が安定となる必要十分条件は数１５の不変零点が安定であることであり、数１６∼数１８においてｈi （ｓ）＝０（ｉ＝１，・・・，ｍ）が全て安定多項式で、（Ｃ，Ａ，Ｂ）の不変零点が安定なことと等価である。【０００９】【数１５】【数１２】【数１３】 10 【数１６】【数１７】【数１４】＊【数１８】 20※ 超平面数９のσ値は正確に求まるが、制御入力数１０の数２１はＣ0i に対応する数２２または数２３を用いざるを得ない。すると、Ｖがリアプノフ関数となるため数１但し、数１９の場合は零点がないため、ｈi （ｓ）＝０４のＫi （ｘ，ｔ）は数２４に修正する必要がある。（ｉ＝１，・・・，ｍ）を安定多項式に選べば良い。【００１２】【００１０】【数２１】【数１９】【数２２】次に、Ｃ行列の不確かさに対する対応について説明する。Ｃ行列が数２０で表される不確かさを持つが、出力 30 ｙは直接に観測できる場合を考える。【００１１】【数２０】【数２３】【数２４】 ※ このように、Ｃ行列の要素となる研磨面の弾性係数と摩擦係数の不確かさに対して、非線形入力ゲインを不確かさが無い場合の所定値より大きくすることで、制御量を 40 安定して超平面に収束させることを可能にしている。【００１３】【発明が解決しようとする課題】しかしながら、従来の制御系では、ロボット１の作業座標系におけるワーク３の位置と摩擦力の制御法の発明に留まり、多関節研磨ロボットを用いて実現するための制御法、即ち関節座標系において制御入力を関節トルクとする場合の制御法については発明されておらず、実用の対象は直交型ロボット等の一部のマニピュレータにおのずと限定されていた。そして直交型マニピュレータでは自由度数の制約から複 50 雑曲面の研磨ができないという欠点を有していた。また、従来の制御系では、スライディングモード非干渉制御を用いて関節角の関数となる項を外乱とみなし、非線形入力ゲインを大きくすることでロバスト化するために、スライディングモード非干渉制御特有のチャタリングが発生し易いという欠点を有していた。【００１４】本発明はこのような従来の課題に鑑みてなされたもので、多関節研磨ロボットにスライディングモード非干渉制御を適用可能としたことで複雑曲面の高精度研磨を可能とし、かつチャタリングの発生を低減したスライディングモード非干渉制御を用いた多関節研磨ロボットを提供することを目的とする。【００１５】 (4) 7 【課題を解決するための手段】このため本発明は、位置及び摩擦力を制御する多関節研磨ロボットであって、多関節研磨ロボットの関節座標系における運動方程式をヤコビアンを用いて関節座標系から作業座標系に変換した多関節研磨ロボットの作業座標系における運動方程式に基づき予め制御設計した制御対象と、作業座標系上の力の目標値と力の検出値間の誤差及び作業座標系上の位置の目標値と位置の検出値間の誤差に基づき多関節研磨ロボット特有の非線形性を補償するスライディングモード非干渉制御手段と、該スライディングモード非干渉制御手段で得た制御入力信号に対し既知のパラメータ変動の逆ダイナミクスを加えた加算信号に基づき多関節研磨ロボットの各関節トルクを算出する関節トルク算出手段と、該関節トルク算出手段で算出した各関節トルクに基づき各関節を駆動する関節駆動手段を備えて構成した。本発明は、制御対象の位置及び摩擦力を制御する多関節研磨ロボットに対し適用する。多関節研磨ロボットの関節座標系における運動方程式を、ヤコビアンを用いて関節座標系から作業座標系に変換する。即ち、多関節研磨ロボットの作業座標系における運動方程式を求める。そして、この運動方程式に基づき制御対象を予め制御設計する。スライディングモード非干渉制御手段では、作業座標系上の力の目標値と力の検出値間の誤差及び作業座標系上の位置の目標値と位置の検出値間の誤差に基づき、多関節研磨ロボット特有の非線形性を補償する。関節トルク算出手段では、スライディングモード非干渉制御手段で得た制御入力信号に基づき、多関節研磨ロボットの各関節トルクを算出する。そして、関節駆動手段では、この算出した各関節トルクに基づき各関節を駆動する。この結果、多関節研磨ロボットにスライディングモード非干渉制御手段を適用することが可能になり、直交型マニピュレータでは自由度数の制約のためになし得なかった複雑曲面の高精度研磨が可能になる。多関節研磨ロボット特有の運動方程式における、関節角の関数項のような既知のパラメータ変動に対しては、その逆ダイナミクスを考慮する。ここで、既知のパラメータ変動は、より具体的には、例えば粘性摩擦、遠心力、コリオリ力及び重力のいずれか少なくとも一つにより生ずるトルクである。そして、スライディングモード非干渉制御の非線形入力は、例えばこれらの未知の変動と同程誤差等を補償するだけにした。このことにより、スライディングモード非干渉制御を用いた多関節研磨ロボットのチャタリングの発生を低減することが可能になった。【００１６】【００１７】【発明の実施の形態】以下、本発明の実施形態を図面に基づいて説明する。本発明の実施形態を図１に示す。図１において、多関節研磨ロボット１０は、垂直２関節マニピュレータ７が同一方向に動作する砥石９（砥石９の代わりに研磨ベルトあるいはバフを用いてもよい）にワ 10 20 特許３２１５８４１ 8 ーク３を押し付け、研磨面に設けた作業座標系においてワーク３の位置と摩擦力を制御することで研磨作業を行うようになっている。【００１８】次に、図１に基づき動作を説明する。垂直２関節マニピュレータ７が、ワーク３をＹop 方向に動作する砥石９（斜線部）に押し当て研磨する。多関節研磨ロボット１０は、作業座標系Σop におけるワーク３の位置Ｐと摩擦力Ｆy が目標値と一致するように制御する。摩擦力Ｆy は垂直２関節マニピュレータ７の第１軸１１と第２軸１３のトルクを制御し押し当て力を調節することで制御する。なお、多関節研磨ロボット１０は２関節で説明したが、本発明の適用は２関節に限定するものではない。【００１９】次に、ヤコビアンを用いて運動方程式を関節座標系から作業座標系に変換する方法について説明する。多関節研磨ロボット１０の第１リンク１５の質量ｍ 1 ，第２リンク１７の質量ｍ2 ，第１軸１１から第２軸１３までの長さｌ1 ，第２軸１３から研磨面までの長さｌ2 ，第１軸１１から第１リンク１５の重心までの長さｒ1 、第２軸１３から第２リンク１７の重心までの長さｒ2 とする。また、多関節研磨ロボット１０のベース座標系をΣB ，作業座標系をΣOP とし、ΣOP におけるΣB の原点の座標値を（ＸA ，ＹA ），ΣOP のＺ軸回りのΣ B の回転角をθA とする。またワーク３の摩擦力をＦy とする。ｎ軸マニピュレータの関節座標系における運動方程式は、数２５で与えられる。【００２０】【数２５】 30 但し、Ｍ（ｑ）（ｎ×ｎ）は慣性行列、ｑ（ｎ×１）は関節角度、数２６は粘性摩擦，遠心力、コリオリ力、重力によって生じるトルク、Ｊr （ｑ）（ｎ×ｎ）は作業座標系と関節座標系を結び付けるヤコビアン、Ｆ（ｎ× １）は研磨面から受ける力、τ（ｎ×１）は関節の入力トルクを表す。【００２１】【数２６】 40 一方、作業座標系におけるワーク３の位置をｐ（ｎ× １）とすると、ｐは関節角ｑの関数であり数２７で表される。【００２２】【数２７】 50 また、数２７の両辺を微分することにより数２８には数２９が成り立つ。【００２３】 (5) 9 特許３２１５８４１ 10 ＊数２９の両辺を微分すると、数３１を得る。【００２５】【数３１】【数２８】【数２９】 T 但し数３０とする。【００２４】【数３０】 10 ＊但し数３３、数３４である。【００２７】【数３３】 ※ 【数３４】 ※ 次に、以下のように外力を定義し、制御系の設計を行う。まず、次のような仮定を行う。力Ｆ（ｎ×１）のうち、研磨面での法線方向力は弾性によって生じ、接線方向の摩擦力は法線方向力に摩擦係数を乗じたものとする。係数行列Ｋ（ｎ×ｎ）を定義すると、ノミナル値と変動を考慮し力Ｆ（ｎ×１）は数３５のように表される。【００２８】【数３５】 30 【数３８】但し数３９である。【００３１】【数３９】ここで状態量を数４０のように定義する。【００３２】【数４０】このとき、Ｋ（ｎ×ｎ）の要素は任意とはならず、物理的性質に対応した構造を有し、後述するように制御の可否に関係する。また、慣性モーメントＭx 、非線形力ｈ x に関してもノミナル値と変動を考慮して数３６、数３７のように定義できる。【００２９】【数３６】 40 また、線形化入力として数４１を定義する。【００３３】【数４１】【数３７】すると運動方程式は数３８のように書き表わせる。【００３０】 -1 数３１を数２５に代入し左から（Ｊr （ｑ））を掛けることにより作業座標系における運動方程式数３２を得る。【００２６】【数３２】すると、数４２の状態方程式が得られる。【００３４】【数４２】 (6) 11 特許３２１５８４１ 12 次に、観測値ｙ（ｌ×ｎ）は接線方向位置と接線方向摩＊ここでｙ1 は力観測成分ベクトル、ｙ2 は位置観測成分擦力からなるが、出力の各成分は互いに従属関係にないベクトルであり、Ｓ1 とＳ2 はｙの各要素が互いに非従ものと仮定する。この仮定は先述した仮定により導き出属関係となるように用途に合わせて選択する選択行列でされる。 10 ある。なお、切り替えゲインは、数４４のように表され【００３５】る。【数４３】【００３６】【数４４】＊但し、数４５である。【００３７】【数４５】 20 ここに、ΔＭ，Δｈ，ΔＫはＭ，ｈ，Ｋの未知の変動量である。マニピュレータ７の関節入力トルクτ（ｔ）は数４６とする。【００３８】【数４６】 30 但し、数４７である。【００３９】【数４７】図２に制御系ブロック線図を示す。スライディングモー 40 ド非干渉制御部２１では、接線方向位置の目標値Ｐr 及び摩擦力の目標値Ｆr を入力する。そして、これらの目標値は、検出された接線方向位置Ｐ及び摩擦力Ｆと比較され、その誤差が求められる。そして、スライディングモード非干渉制御に基づく演算がなされ、制御入力信号ｕが出力される。制御入力信号ｕに対しては、非線形力ｈx0 の逆ダイナミクスが加算される。この加算信号はヤコビアンにより関節トルクに変換されて、垂直２関節マニピュレータ７ヘ入力される。【００４０】また、図３に摩擦係数α。＝０．５，弾性 50 係数ｋ。＝１０００（Ｎ／ｍ），α。，ｋ。，ＭX0 の変動を１０％としたときのシミュレーション結果を示す。実線は摩擦カＦy ，点線は目標値Ｆry ，破線はワーク３のＹ方向の位置Ｐy ，目標値Ｐry ＝０である。この例からＦy ，Ｐy は目標値に追従し、パラメータ変動に対し、ロバストなハイブリッド制御系が実現されていることがわかる。【００４１】以上のように、マニピュレータ７の運動方程式を、ヤコビアンを用いて関節座標系から作業座標系に変換することで、多関節研磨ロボットの位置・摩擦力制御にスライディングモード非干渉制御を適用可能にし、多自由度研磨ロボットの高精度な位置・摩擦力制御を可能とすることが出来る。また、マニピュレータ７特有の運動方程式における関節角の関数項のような既知のパラメータ変動に対してはその逆ダイナミクスを考慮し、スライディングモード非干渉制御の非線形入力は未知のパラメータ変動を補償するだけにした。従って、スライディングモード制御特有のチヤタリングの発生を低減することが出来る。【００４２】【発明の効果】以上説明したように本発明によれば、マニピュレータの運動方程式を、関節座標系から作業座標系に変換したことで、多関節研磨ロボットにスライディングモード非干渉制御手段を適用することが可能になり、直交型マニピュレータでは自由度数の制約のためになし得なかった複雑曲面の高精度研磨が可能になった。そして、スライディングモード非干渉制御への非線形入力は、未知のパラメータ変動のみとしたことにより、スライディングモード非干渉制御を用いた多関節研磨ロボットのチャタリングの発生を低減することが可能になった。【００４３】 (7) 13 【００４４】【図面の簡単な説明】【図１】本発明の実施形態の構成図【図２】制御系ブロック線図【図３】シミュレーション結果【符号の説明】１ロボット３ワーク【図１】特許３２１５８４１ 14 ＊７垂直２関節マニピュレータ９砥石１０多関節研磨ロボット１１第１軸１３第２軸１５第１リンク１７第２リンク＊２１スライディングモード非干渉制御部【図２】【図３】 ───────────────────────────────────────────────────── フロントページの続き (56)参考文献特開特開特開特開特開平９−103945（ＪＰ，Ａ) 平７−121209（ＪＰ，Ａ) 昭63−47058（ＪＰ，Ａ) 平５−88749（ＪＰ，Ａ) 平２−297612（ＪＰ，Ａ) 7 (58)調査した分野(Int.Cl. ，ＤＢ名) B25J 13/00 B25J 9/16 B24B 27/00 G05B 13/00 G05D 3/12 305