JPH07299778A - ロボット制御装置 - Google Patents

Abstract

(57)【要約】 【目的】ロボットアームの反力に含まれる不要な要素を
取除くと共に、ロボットアームの作業状況に対応して変
化量等を変更するようにする。 【構成】設定部(52)より入力された目標軌道にロボット
アーム(12)が移動するように駆動制御部(51)が上記ロボ
ットアーム(12)を制御する。そして、上記目標軌道に正
弦波状の変化量を加える摂動部(53)と、上記ロボットア
ーム(12)の反力を検出する力センサ(40)とが設けられて
いる。更に、上記変化量と反力との積の積分値であっ
て、変化量のπ／２から５π／２の区間に亘って積分し
た評価関数を演算する評価演算部(54)が設けられてい
る。この評価関数に基づいて目標軌道の修正量を演算し
て該目標軌道に入力するフィードバック演算部(55)が設
けられている。加えて、上記反力に基いてパラメータの
変更信号を上記摂動部(53)、評価演算部(54)及びフィー
ドバック演算部(55)に出力するパラメータ演算部(56)が
設けられている。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボット制御装置に関
し、特に、正弦波摂動適応制御対策に係るものである。

【０００２】

【従来の技術】従来より、ロボットの制御方法の１つと
して力フィードバックによる適応制御方法があり、例え
ば、特開昭６３−１５０１８５号公報に開示されている
ものがある。この制御装置は、設定部より制御目標値が
サーボモータに入力される一方、該サーボモータで駆動
される指に角度センサを設け、この角度センサからの検
出信号に基いて指の回転角度と回転速度とを演算し、回
転角度と回転速度との修正量を演算してフィードバック
演算部が制御目標値に修正量をフィードバックするよう
にしている。一般的には、図16に示すように、設定部(9
0)より制御目標値がロボット制御部(91)に入力され、該
ロボット制御部(91)が制御対象部(92)を制御して該制御
対象部(92)が組立等の作業を行うことになる。そして、
上記制御対象部(92)の反力を力センサ(93)が検出し、こ
の力センサ(93)の検出信号に基いてフィードバック演算
部(94)が修正量を演算して該修正量を制御目標値にフィ
ードバックする。これによって、上記ロボット制御部(9
1)が制御対象部(92)をフィードバック制御するようにし
ている。

【０００３】

【発明が解決しようとする課題】上述した力フィードバ
ックによる適応制御方法では、ノイズの影響が大きく、
組立作業に必ずしも最適であるとはいえず、且つ有効な
情報が得られ難いという問題があった。そこで、ノイズ
の影響が小さく、組立作業に適し、且つ有効な情報を得
ることができる正弦波摂動適応制御システムが考えられ
ている。この正弦波摂動適応制御システムは、図17に示
すように、系の入力である設定部(90)からの制御目標値
に摂動部(95)が既知の正弦波状の変化量、つまり、正弦
波を加え、この正弦波と、力センサ(93)の検出反力であ
る正弦波の応答とから評価演算部(96)が評価関数を計算
し、この評価関数からフィードバック演算部(96)が適切
な修正量を制御目標値にフィードバックする方法であ
る。しかしながら、この正弦波摂動適応制御システム
は、対象とする系の応答（反力）が正弦波のみに反応す
ると考えて制御しているので、この対象とする系の応答
が正弦波以外の要素、例えば、時間や系の状態の影響に
よって変化する場合、系を正確に制御することができな
いという問題がある。また、系に加える正弦波は、一定
の特性のみを有する波形であり、つまり、作業状況の変
化に拘りなく、一定の正弦波であり、また、作業状況に
拘りなく、反力のみに依存して修正量を演算しているの
で、組立作業等を行う場合、制御対象部の変動が大きく
なり過ぎる場合があり、部品が塑性変形したり、部品が
破損する可能性があるという問題がある。

【０００４】本発明は、斯かる点に鑑みてなされたもの
で、制御対象部の応答量に含まれる不要な要素を取除く
ようにして制御精度の向上を図ると共に、制御対象部の
作業状況に対応して変化量等を変更するようにし、部品
の破損等を防止することを目的とするものである。

【０００５】

【課題を解決するための手段】上記の目的を達成するた
めに、本発明が講じた手段は、評価値の積分算出区間を
変更する一方、変化量の波形等を変更するようにしたも
のである。具体的に、図１に示すように、請求項１に係
る発明が講じた手段は、先ず、設定部(52)より入力され
た制御目標値に制御対象部(12)が移動するように駆動制
御部(51)が上記制御対象部(12)を制御するロボット制御
装置を前提としている。そして、上記駆動制御部(51)に
入力する制御目標値に正弦波状に変動する変化量を加え
る摂動手段(53)と、上記駆動制御部(51)に入力する制御
目標値に対応した制御対象部(12)の応答量を検出する検
出手段(40)とが設けられている。更に、上記摂動手段(5
3)が加えた変化量と上記検出手段(40)が検出した応答量
との積の積分値であって、正弦波状の変化量の最大振幅
点又は最小振幅点から１周期又は複数周期の区間に亘っ
て積分した評価値を演算する評価手段(54)が設けられて
いる。加えて、該評価手段(54)の評価値に基づいて制御
目標値の修正量を演算して該制御目標値に入力するフィ
ードバック手段(55)が設けられている。

【０００６】また、請求項２に係る発明が講じた手段
は、先ず、設定部(52)より入力された制御目標値に制御
対象部(12)が移動するように駆動制御部(51)が上記制御
対象部(12)を制御するロボット制御装置を前提としてい
る。そして、上記駆動制御部(51)に入力する制御目標値
に正弦波状に変動する変化量を加える摂動手段(53)と、
上記駆動制御部(51)に入力する制御目標値に対応した制
御対象部(12)の応答量を検出する検出手段(40)とが設け
られている。更に、上記摂動手段(53)が加えた変化量と
上記検出手段(40)が検出した応答量との積を変化量の所
定入力区間に亘って積分した評価値を演算する評価手段
(54)が設けられている。加えて、該評価手段(54)の評価
値に基づいて制御目標値の修正量を演算して該制御目標
値に入力するフィードバック手段(55)が設けられてい
る。その上、上記検出手段(40)が検出した応答量に基い
てパラメータの変更信号を少なくとも上記摂動手段(53)
及び評価手段(54)とフィードバック手段(55)との何れか
一方に出力する変更手段(56)が設けられている。

【０００７】また、請求項３に係る発明が講じた手段
は、請求項１の発明において、請求項２の発明における
変更手段(56)を設けたものである。また、請求項４に係
る発明が講じた手段は、上記請求項１〜３の何れか１の
発明において、検出手段(40)が、制御対象部(12)の動作
反力を検出する力センサで構成されたものである。ま
た、請求項５に係る発明が講じた手段は、上記請求項２
〜４の何れか１の発明において、変更手段(56)が、正弦
波状の変化量の正弦波形を変更させる波形変更信号を出
力するように構成されたものである。また、請求項６に
係る発明が講じた手段は、上記請求項２〜４の何れか１
の発明において、変更手段(56)が、フィードバック手段
(55)の修正量を変更させる修正量変更信号を出力するよ
うに構成されたものである。

【０００８】

【作用】上記の構成により、請求項１及び３に係る発明
では、先ず、設定部(52)より制御目標値が駆動制御部(5
1)に入力され、この制御目標値に従って駆動制御部(51)
は制御対象部(12)を制御し、該制御対象部(12)が制御目
標値に移動することになる。一方、検出手段(40)は、制
御目標値に対応した制御対象部(12)の応答量を検出して
おり、具体的に、請求項４に係る発明では、力センサ(4
0)が、制御対象部(12)の反力を検出している。一方、上
記応答量が、例えば、所定の閾値を越えると、移動抵抗
が大きいことになり、摂動手段(53)が制御目標値に変化
量である正弦波を加えることになる。そして、この変化
量が最大振幅点又は最小振幅点になると、評価手段(54)
が変化量と応答量との積の積分を変化量の１周期又は複
数周期に亘って演算して評価値を算出する。具体的に、
上記評価手段(54)は、例えば、変化量の正弦波のπ／２
から５π／２の区間に亘って積分を行うことになる。そ
の後、上記評価手段(54)が演算した評価値に基づいてフ
ィードバック手段(55)は、修正量を演算して制御目標値
に入力することになり、この新たな制御目標値に基づい
て駆動制御部(51)は制御対象部(12)を制御することにな
る。この結果、上記応答量が正弦波状の変化量以外の要
因で変化してもこの不要な要因が除去されることにな
る。

【０００９】また、請求項２及び３に係る発明では、検
出手段(40)が検出する応答量が、例えば、所定の閾値を
越えると、制御対象部(12)の作業状況が変化したことに
なり、この応答量に基づいて変更手段(56)がパラメータ
の変更信号を出力することになる。つまり、請求項５に
係る発明では、上記変更手段(56)は、変化量の振幅又は
周期を小さくする波形変更信号を摂動手段(53)及び評価
手段(54)に出力し、制御対象部(12)の変化を小さくす
る。また、請求項６に係る発明では、上記変更手段(56)
は、修正量を小さくする修正量変更信号をフィードバッ
ク手段(55)に出力し、制御対象部(12)の変化を小さくす
る。この結果、作業部品の変形や破損が未然に防止され
る。

【００１０】

【発明の効果】従って、請求項１及び３に係る発明によ
れば、評価手段(54)の積分区間を正弦波状の変化量の最
大振幅点又は最小振幅点から１周期又は複数周期とした
ゝめに、制御対象部(12)の応答が正弦波状の変化量以外
の要素、例えば、時間や系の状態の影響によって変化す
る場合においてもこの時間等の影響を取り除くことがで
きる。この結果、制御対象部(12)を正確に制御すること
ができる。更に、上記制御対象部(12)が作業を継続する
状態で制御目標値を修正することができることから、制
御対象部(12)の制御を円滑に且つ迅速に制御することが
できる。また、請求項２及び３に係る発明によれば、上
記正弦波状の変化量又は修正量を変化させるようにした
ゝめに、部品の塑性変形や破損等を確実に防止すること
ができる。つまり、請求項５に係る発明によれば、変化
量の振幅等を小さくして該変化量による部品の変動を小
さくしすることができ、請求項６に係る発明によれば、
修正量を小さくして該修正量による部品の変動を小さく
することができるので、部品の破損等を確実に防止する
ことができる。また、請求項４に係る発明によれば、力
センサが応答量として反力を検出するようにしたゝめ
に、１つの歪みゲージ等を設けるのみでもって不要な要
素を除去することができ、正確な制御を実行することが
できる。

【００１１】

【実施例】以下、本発明の実施例を図面に基づいて詳細
に説明する。図２に示すように、ロボット(10)は、熱交
換器(20)の組立作業に適用されており、該熱交換器(20)
は、例えば、空気調和装置に設けられるものであって、
多数のフィン(21,21, … )に銅管からなるヘアピン(22)
が挿入固定されて構成されている。そして、該熱交換器
(20)は、ヘアピン(22)の内部を冷媒が流通して熱交換す
るように構成されている。上記ロボット(10)は、ロボッ
ト本体(11)にアーム(12)が開動自在に連結されて成り、
該アーム(12)は先端にハンド(13)を備えて制御対象部に
構成されている。そして、該ハンド(13)は、上記ヘアピ
ン(22)を把持し、テーブル(30)に固定金具(31)によって
固定されたフィン(21,21, … )にヘアピン(22)を挿入す
る作業を行うことになる。また、上記アーム(12)の先端
部には、力センサ(40)が設けられており、ヘアピン(22)
を挿入する軌道方向Ｚの反力であるＺ方向力Fzと、該軌
道方向Ｚに直交する直交方向Ｙの反力であるＹ方向力Fy
とを検出する検出手段を構成している。

【００１２】上記ロボット本体(11)には、コントローラ
(50)が設けられ、該コントローラ(50)は、図３に示すよ
うに、駆動制御部(51)が設けられている。そして、該駆
動制御部(51)が、図示しないが、サーボモータ等を制御
して制御対象部であるアーム(12)の移動を制御するよう
に構成されている。上記コントローラ(50)は、本発明の
特徴として、正弦波摂動適応制御システムを採用してお
り、図４に示すように、設定部(52)より上記駆動制御部
(51)に入力される制御目標値である目標軌道指令X1を力
センサ(40)が検出したＺ方向力Fzに基づいて目標軌道Ｘ
に修正するように構成されている。また、上記コントロ
ーラ(50)は、摂動部(53)を備えると共に、評価値である
評価関数を演算する評価演算部(54)とフィードバック演
算部(55)とが設けられている。上記摂動部(53)は、正弦
波状の変化量（以下、単に正弦波という。）を目標軌道
Ｘに加える摂動手段を構成している。上記評価演算部(5
4)は、本発明の特徴の１つであって、摂動部(53)が加え
た正弦波（変化量）と上記力センサ(40)が検出したＺ方
向力Fzとの積の積分値であって、正弦波の最大振幅点か
ら１周期の区間に亘って積分した評価関数を算出する評
価手段を構成している。つまり、上記評価演算部(54)
は、摂動部(53)が加える正弦波のπ／２から５π／２の
区間で積分するように構成されている。上記フィードバ
ック演算部(55)は、評価演算部(54)の評価関数に基づい
て目標軌道Ｘの修正量を演算して該目標軌道Ｘに入力す
るフィードバック手段を構成している。

【００１３】そこで、上記正弦波摂動適応制御システム
において、評価演算部(54)の積分区間を［π／２，５π
／２］にした基本的原理について説明する。先ず、ロボ
ット(10)の目標軌道指令をX1とすると、正弦波を加えた
時の時間ｔにおける駆動制御部(51)に入力される実際の
目標軌道Ｘは、正弦波の振幅をε、周期をωとすると、

【００１４】

【式１】 となる。また、上記評価演算部(54)が演算する評価関数
Ｒは、評価する応答として力を選び、力センサ(40)から
得られる力の応答、つまり、Ｚ方向力FzをＦ(t) とする
と、

【００１５】

【式２】 となる。一方、上記ロボット(10)が行う挿入作業の場
合、ロボット(10)の挿入軌道である目標軌道Ｘと挿入軌
道方向の反力であるハンド(13)のＺ方向力Fzとは、図５
に示すような特性曲線Ｌの関係がある。そして、上記力
Ｆ(t) は、挿入軌道Ｘの関数Ｆ(X) として表されるの
で、上記(2)式は、次式の通りとなる。

【００１６】

【式３】 また、上記力Ｆ(X) は、各挿入軌道Ｘの近傍では、

【００１７】

【式４】 となり、図５において、現在の挿入軌道（目標軌道指令
X1）をP1とすると、上記(3)式は、 (4)式及び (1)式を
代入し、

【００１８】

【式５】 となる。

【００１９】つまり、αを次式の通りに設定すると、

【００２０】

【式６】 上記 (5)式は、 Ｒ＝αＫ ……(7) と成り、上記評価関数Ｒは、特性曲線Ｌの傾きＫの関数
で表され、つまり、傾きＫに比例した値となる。そし
て、この評価関数Ｒにゲインを掛けて修正量を導出し、
目標軌道Ｘにフィードバックすることによって挿入軌道
をXcに収束させることができる。この結果、手先反力で
あるＺ方向力Fz（挿入抵抗）が最小になる状態で挿入作
業が行われるようになる。

【００２１】この正弦波摂動適応制御システムにおい
て、上記評価関数Ｒを演算するに当り、従来において
は、 (5)式に示すように、積分区間を正弦波の［０，２
π］としていた。しかしながら、上記図５に示す特性曲
線Ｌが時間に対して一定である場合には何ら問題はない
が、上記特性曲線Ｌが時間に対して変化する場合には、
上記(7)式が成り立たなくなり、適切な修正量を算出す
ることができなくなる。そこで、例えば、図５における
P3について考察すると、つまり、Ｚ方向力Fzが最小値と
なる点P3について考察すると、図５の特性曲線Ｌが、時
間ｔに対して一定である場合、この図５のP3において
は、正弦波を加えてもＺ方向力Fzは変化することがな
く、時間ｔに対するP3のＺ方向力Ｆzcは、図６に示すよ
うに、一定となる。従って、上記評価関数Ｒを (3)式に
基づいて計算すると、

【００２２】

【式７】 となり、上記 (7)式を充足することになる。一方、図５
の特性曲線Ｌが、時間ｔに比例して上方に移動するとす
ると、図５のP3におけるＺ方向力Ｆzcは、図７に示すよ
うに、正弦波とは無関係にβに比例して変化することに
なる。この場合において、上記評価関数Ｒを (3)式に基
づいて計算すると、次式に示すように、

【００２３】

【式８】 となり、上記 (7)式を満たさなくなる。

【００２４】そこで、本発明では、上記評価関数Ｒを演
算するに当り、積分区間を正弦波に対して［π／２，５
π／２］に変更することにより、図５の特性曲線Ｌが移
動した際の評価関数Ｒに対する影響をなくするようにし
ている。具体的に、図５におけるP3について考察する
と、図５の特性曲線Ｌが、時間ｔに対して一定である場
合、上記評価関数Ｒを (3)式に基づいて計算すると、次
式に示すように、

【００２５】

【式９】 となり、上記 (7)式を充足することになる。一方、図５
の特性曲線Ｌが、時間ｔに比例して上方に移動するとす
ると、図５のP3におけるＺ方向力Ｆzcは、図７に示すよ
うに、正弦波とは無関係にβに比例した変化線Ｍに沿っ
て変化することになり、この場合の上記評価関数Ｒを
(3)式に基づいて計算すると、次式に示すように、

【００２６】

【式１０】 となり、上記(10)式と同様に上記 (7)式を充足すること
になる。つまり、図５の特性曲線Ｌが、時間ｔに比例し
て上方に移動する場合、図５のP1及びP2では、図７の変
化線Ｍ上に正弦波加振による力の応答が乗ることにな
り、図８及び図９に示すように、評価関数Ｒに対する特
性曲線Ｌの移動による影響がなくなり、正弦波に対する
応答を正確に評価することになる。その上、図５のP1に
おいては、図８にも示すように、Ｚ方向力Ｆz1は、正弦
波に対して凸、凹及び凸の順に変化し、評価関数Ｒは
（＋）となり、図５のP2においては、図９にも示すよう
に、Ｚ方向力Ｆz2は、逆に正弦波に対して凹、凸及び凹
の順に変化し、評価関数Ｒは（−）となる。以上の理由
から、従来、図10に示すように、目標軌道Ｘに対して正
弦波を１周期（２π）加えると共に、積分区間を［０，
２π］としていたのに対し、本実施例では、図11に示す
ように、目標軌道Ｘに対して正弦波を１周期半（３π）
加えると共に、積分区間を［π／２，５π／２］として
いる。

【００２７】また、本発明の他の特徴として、上記コン
トローラ(50)はパラメータ演算部(56)を備えている。該
パラメータ演算部(56)は、力センサ(40)が検出したＹ方
向力Fyに基づいてパラメータの変更信号を上記摂動部(5
3)及び評価演算部(54)に出力すると共に、フィードバッ
ク演算部(55)に出力する変更手段を構成している。具体
的に、上記パラメータ演算部(56)は、正弦波状の変化量
である正弦波の周期ω及び振幅εを変更させる波形変更
信号を摂動部(53)及び評価演算部(54)に出力すると共
に、修正量を変更させる修正量変更信号、つまり、フィ
ードバック演算部(55)の制御ゲインを変更させる修正量
変更信号をフィードバック演算部(55)に出力するように
構成されている。

【００２８】そこで、上記正弦波の周期ωや振幅ε等を
変更するようにした理由について説明する。従来、正弦
波の振幅εや周期ωは一定であり、また、フィードバッ
クの制御ゲインKfは軌道方向ＺのＺ方向力Fzによって定
められていた。しかしながら、組立作業等の場合、作業
の進行状況によって部品の状態や環境が変化し、正弦波
の振幅ε等が一定の場合、部品の状態を正確に評価する
ことができない。具体的に、図12に示すように、ヘアピ
ン(22)の挿入初期においては、該ヘアピン(22)がフィン
(21,21, … )に僅かに挿入されているのみであるので、
ヘアピン(22)の許容撓み量は大きく、ハンド(13)は直交
方向Ｙに大きく変位することができる。しかし、図13に
示すように、ヘアピン(22)の挿入終期においては、該ヘ
アピン(22)がフィン(21,21, … )にほゞ挿入され終わっ
ているので、挿入初期のように非挿入部分が長い状態の
場合と同様に揺することができない。無理に大きな振動
を加えると、ヘアピン(22)の許容撓み量が小さいので、
ヘアピン(22)が塑性変形したり、破損することになる。
更に、評価関数Ｒがほゞ適正に算出されても、非挿入部
分が短い状態において（図13参照）、フィードバックさ
れる修正量が大きすぎると、ヘアピン(22)が破損した
り、ロボット(10)の動作が不安定になる。そこで、本発
明では、ヘアピン(22)の挿入状態に応じて正弦波の振幅
ε等を変更するようにしたものである。具体的に、上記
ヘアピン(22)の挿入初期においては、抵抗であるＹ方向
力Fyが小さいので、このＹ方向力Fyが小さいと、ヘアピ
ン(22)の非挿入部分が長いと判定し、正弦波の周期ω及
び振幅εを大きくすると共に、フィードバック演算部(5
5)の制御ゲインKfを大きくする一方、上記ヘアピン(22)
の挿入終期においては、抵抗であるＹ方向力Fyが大きい
ので、このＹ方向力Fyが大きいと、ヘアピン(22)の非挿
入部分が短いと判定し、正弦波の周期ω及び振幅εを小
さくすると共に、フィードバック演算部(55)の制御ゲイ
ンKfを小さくする。

【００２９】−ロボット(10)の制御動作− そこで、上記ロボット(10)の制御動作について、図14の
制御フローに基づいて説明する。

【００３０】該ロボット(10)のコントローラ(50)におい
て、設定部(52)より目標軌道Ｘが駆動制御部(51)に入力
され、この目標軌道Ｘに従って駆動制御部(51)はアーム
(12)及びハンド(13)を制御し、ハンド(13)がヘアピン(2
2)を把持してフィン(21,21,… )に挿入することにな
る。一方、力センサ(40)は、ハンド(13)がヘアピン(22)
をフィン(21,21, … )に挿入する際の挿入軌道方向Ｚで
あるＺ方向力Fz（反力）と直交方向ＹのＹ方向力Fy（反
力）とを検出しており、このＺ方向力Fzは評価演算部(5
4)に入力される一方、Ｙ方向力Fyはパラメータ演算部(5
6)に入力されている。そこで、先ず、ステップST１にお
いて、Ｚ方向力Fzが予め設定された閾値（スレッショル
ド レベル）Ａより大きいか否かが判定され、該Ｚ方向
力Fzが閾値Ａ以下である場合、挿入抵抗が小さいことに
なり、スムーズにヘアピン(22)がフィン(21,21, … )に
挿入されていることになるので、フィードバック制御は
終了してそのまゝの目標軌道Ｘでヘアピン(22)を挿入し
続けることになる。

【００３１】上記Ｚ方向力Fzが閾値Ａより大きい場合、
ヘアピン(22)の挿入抵抗が大きいことになり、スムーズ
にヘアピン(22)がフィン(21,21, … )に挿入されていな
いことになるので、上記ステップST１の判定がＹＥＳと
なってステップST２に移り、タイマｔと評価関数Ｒとを
初期化してステップST３に移り、パラメータ計算を行う
ことになる。このパラメータ計算は、図15に示すように
行われ、この動作については後述する。

【００３２】続いて、ステップST４に移り、正弦波の計
算を行い、つまり、摂動部(53)が目標軌道Ｘに変化量で
ある正弦波を加えるので、dx＝ε×Sin(ωｔ）を計算
し、目標軌道Ｘが(1) 式の通りであるので、この目標軌
道Ｘを、Ｘ＝X1＋dxに基づいて算出する（X1は目標軌道
指令）。その後、ステップST５に移り、タイマｔがπ／
２から５π／２の区間に入っているか否かを判定し、つ
まり、積分区間に入っているか否かを判別する。上記正
弦波を加えた初期においては、π／２に入っていないの
で、ステップST５の判定がＮＯとなってステップST６に
移り、上記タイマｔが３πになったか否かを判定し、つ
まり、１周期半の正弦波を加え終わったか否かを判定す
る。そして、１周期半の正弦波を加え終わっていない場
合、上記ステップST６の判定がＮＯとなってステップST
７に移り、上記タイマｔに微小時間dtを加えてステップ
ST４に戻ることになる。

【００３３】その後、上述した動作を繰返し、積分区間
に入ると、上記ステップST５の判定がＹＥＳとなってス
テップST８に移り、評価演算部(54)が評価関数Ｒを計算
することになる。つまり、微小時間毎に評価関数Ｒを次
式に基づいて計算し、 Ｒ＝Fz×dx＋R1 ……(12) この現在の評価関数Ｒを次回の計算に用いる前回の評価
関数R1に設定する。以後、上述したステップST７に移
り、上記タイマｔが５π／２になるまで、上記ステップ
ST８の評価関数Ｒの計算を繰返すことになる。つまり、
(3)式の積分計算を行うことになる。

【００３４】上記タイマｔが５π／２になると、積分区
間が終了するので、上記ステップST５の判定が再びＮＯ
となってステップST６に移り、上記タイマｔが３πにな
ったか否かを判定し、つまり、図11に示すように、正弦
波の加振期間と積分区間とは一致していないので、１周
期半の正弦波を加え終わるまで、評価関数Ｒを計算する
ことなく上述のステップST４からステップST７にまでの
動作を繰返すことになる。その後、上記タイマｔが３π
になると、上記ステップST６の判定がＹＥＳとなってス
テップST９に移り、フィードバック演算部(55)が評価関
数Ｒに所定の制御ゲインKfを掛けて修正量を算出し、目
標軌道X1に加えて修正した新たな目標軌道Ｘが駆動制御
部(51)に入力されてアーム(12)が制御されることにな
る。そして、この新たな目標軌道Ｘは、次回の修正に用
いるために前回の目標軌道X1に設定され、１回の目標軌
道Ｘの修正が終了する。この目標軌道Ｘの修正を繰返
し、Ｚ方向力Fzを最小値に収束させ、つまり、図５に示
すP3、或いはこのP3の近傍でヘアピン(22)をフィン(21,
21, … )に挿入させることになる。

【００３５】次に、図15に示すパラメータ計算の制御動
作について説明する。先ず、上記ステップST３に移る
と、ステップST31において、Ｙ方向力Fyが予め設定され
た閾値Ｂより大きいか否かを判定し、該Ｙ方向力Fyが閾
値Ｂ以下である場合、図12に示すように、ヘアピン(22)
の挿入初期で、非挿入部分が長いと判定し、そのまゝ上
記ステップST４に移ることになる。上記Ｙ方向力Fyが閾
値Ｂより大きい場合、図13に示すように、ヘアピン(22)
の挿入終期で、非挿入部分が短いと判定し、上記ステッ
プST31の判定がＹＥＳとなってステップST32に移り、パ
ラメータ演算部(56)がパラメータを計算する。つまり、
上記ヘアピン(22)の挿入初期における修正量のゲインを
k1、正弦波の振幅εのゲインをk2及び正弦波の周期、つ
まり、角速度ωのゲインをk3とすると、各ゲインk1、k2
及びk3をＹ方向力Fyで割って、次式の通り、新たな修正
量の制御ゲインKf、正弦波の振幅ε及び正弦波の角速度
ωを算出して上記ステップST４に移ることになる。 Kf＝（k1／Fy）×Kf ……(13) ε＝（k2／Fy）×ε ……(14) ω＝（k3／Fy）×ω ……(15) そして、この制御ゲインKfはフィードバック演算部(55)
に、正弦波の振幅ε及び正弦波の角速度ωは摂動部(53)
及び評価演算部(54)に出力され、修正量が小さくなり、
しかも、正弦波の振幅及び周期が小さくなる。

【００３６】−実施例の効果− 従って、本実施例によれば、上記評価演算部(54)の積分
区間を正弦波の加振期間に対してπ／２から５π／２ま
でとしたゝめに、制御対象部の反力が正弦波以外の要
素、例えば、時間や系の状態の影響によって変化する場
合においてもこの時間等の影響を取り除くことができ
る。この結果、制御対象部を正確に制御することができ
る。更に、上記アーム(12)が作業を継続する状態で目標
軌道を修正することができることから、アーム(12)の制
御を円滑に且つ迅速に制御することができる。また、上
記力センサ(40)が検出するＺ方向力Fzによって時間等の
不要な要素を除去することができるので、１つの歪みゲ
ージ等を設けるのみでもって正確な制御を実行すること
ができる。また、上記正弦波及び修正量を作業状況に対
応して変化させるようにしたゝめに、ヘアピン(22)の破
損等を確実に防止することができる。つまり、ヘアピン
(22)の挿入初期においては正弦波の振幅等を大きくし、
Ｚ方向力Fzの応答を十分に得ることができると同時に、
修正量を大きくして収束性を向上させることができる一
方、ヘアピン(22)の挿入終期においては正弦波の振幅等
を小さくし、該正弦波等による変動を小さくすることが
できるので、ヘアピン(22)の塑性変形や破損等を確実に
防止することができる。

【００３７】−その他の変形例− 尚、本実施例においては、熱交換器(20)の組立作業を行
うロボット(10)について説明したが、本発明は、熱交換
器(20)の組立作業に限られず、空気調和装置や冷凍機な
どの各種組立作業のロボットの他、各種の作業を行うロ
ボットに適用してもよいことは勿論である。また、上記
評価演算部(54)の積分区間は、正弦波の最小振幅点から
開始するようにしてもよく、また、１周期に限られず、
複数周期に亘る積分を演算するようにしてもよい。つま
り、例えば、３周期の積分を行い、１周期毎の積分の平
均値を算出するようにしてもよい。また、請求項１の発
明では、評価演算部(54)の積分区間を正弦波のπ／２か
ら５π／２まで等とするのみであってもよく、また、請
求項２の発明では、パラメータの変更のみを行うように
してもよい。また、パラメータ演算部(56)は、正弦波の
振幅εのみを変更するようにしてもよく、また、正弦波
の周期ωのみを変更するようにしてもよく、更にまた、
修正量の制御ゲインKfのみを変更するようにしてもよ
い。

【００３８】また、本実施例では、検出手段は応答量と
して力を検出する力センサ(40)を用いたが、位置センサ
などであってもよく、つまり、系に入力する正弦波と相
関関係にある応答量を検出するものであればよい。

【図面の簡単な説明】

【図１】本発明の構成を示すブロック図である．

【図２】ロボットの概略平面図である。

【図３】ロボットの制御系を示すブロック図である。

【図４】ロボットのアーム先端部を示す拡大図である。

【図５】目標軌道に対するＺ方向力の特性図である。

【図６】特性曲線が一定の場合を示すＺ方向力の特性図
である。

【図７】特性曲線が変化する場合を示すＺ方向力の特性
図である。

【図８】特性曲線が変化する場合を示す図５P1のＺ方向
力の特性図である。

【図９】特性曲線が変化する場合を示す図５P2のＺ方向
力の特性図である。

【図１０】従来の積分区間を示す正弦波の波形図であ
る。

【図１１】本発明の積分区間を示す正弦波の波形図であ
る。

【図１２】作業初期を示すロボットの要部の平面図であ
る。

【図１３】作業終期を示すロボットの要部の平面図であ
る。

【図１４】目標軌道の修正制御を示す制御フロー図であ
る。

【図１５】パラメータ変更の制御動作を示す制御フロー
図である。

【図１６】従来のフィードバック制御システムを示す制
御ブロック図である。

【図１７】従来の正弦波摂動適応制御システムを示す制
御ブロック図である。

【符号の説明】

10 ロボット 12 アーム 40 力センサ（検出手段） 50 コントローラ 51 駆動制御部 52 設定部 53 摂動部（摂動手段） 54 評価演算部（評価手段） 55 フィードバック演算部（フィードバッ
ク手段） 56 パラメータ演算部（変更手段）

Claims (6)

【特許請求の範囲】
1. 【請求項１】 設定部(52)より入力された制御目標値に
   制御対象部(12)が移動するように駆動制御部(51)が上記
   制御対象部(12)を制御するロボット制御装置において、 上記駆動制御部(51)に入力する制御目標値に正弦波状に
   変動する変化量を加える摂動手段(53)と、 上記駆動制御部(51)に入力する制御目標値に対応した制
   御対象部(12)の応答量を検出する検出手段(40)と、 上記摂動手段(53)が加えた変化量と上記検出手段(40)が
   検出した応答量との積の積分値であって、正弦波状の変
   化量の最大振幅点又は最小振幅点から１周期又は複数周
   期の区間に亘って積分した評価値を演算する評価手段(5
   4)と、 該評価手段(54)の評価値に基づいて制御目標値の修正量
   を演算して該制御目標値に入力するフィードバック手段
   (55)とを備えていることを特徴とするロボット制御装
   置。
2. 【請求項２】 設定部(52)より入力された制御目標値に
   制御対象部(12)が移動するように駆動制御部(51)が上記
   制御対象部(12)を制御するロボット制御装置において、 上記駆動制御部(51)に入力する制御目標値に正弦波状に
   変動する変化量を加える摂動手段(53)と、 上記駆動制御部(51)に入力する制御目標値に対応した制
   御対象部(12)の応答量を検出する検出手段(40)と、 上記摂動手段(53)が加えた変化量と上記検出手段(40)が
   検出した応答量との積を変化量の所定入力区間に亘って
   積分した評価値を演算する評価手段(54)と、 該評価手段(54)の評価値に基づいて制御目標値の修正量
   を演算して該制御目標値に入力するフィードバック手段
   (55)と、 上記検出手段(40)が検出した応答量に基づいてパラメー
   タの変更信号を少なくとも上記摂動手段(53)及び評価手
   段(54)とフィードバック手段(55)との何れか一方に出力
   する変更手段(56)とを備えていることを特徴とするロボ
   ット制御装置。
3. 【請求項３】 設定部(52)より入力された制御目標値に
   制御対象部(12)が移動するように駆動制御部(51)が上記
   制御対象部(12)を制御するロボット制御装置において、 上記駆動制御部(51)に入力する制御目標値に正弦波状に
   変動する変化量を加える摂動手段(53)と、 上記駆動制御部(51)に入力する制御目標値に対応した制
   御対象部(12)の応答量を検出する検出手段(40)と、 上記摂動手段(53)が加えた変化量と上記検出手段(40)が
   検出した応答量との積の積分値であって、正弦波状の変
   化量の最大振幅点又は最小振幅点から１周期又は複数周
   期の区間に亘って積分した評価値を演算する評価手段(5
   4)と、 該評価手段(54)の評価値に基づいて制御目標値の修正量
   を演算して該制御目標値に入力するフィードバック手段
   (55)と、 上記検出手段(40)が検出した応答量に基づいてパラメー
   タの変更信号を少なくとも上記摂動手段(53)及び評価手
   段(54)とフィードバック手段(55)との何れか一方に出力
   する変更手段(56)とを備えていることを特徴とするロボ
   ット制御装置。
4. 【請求項４】 請求項１乃至３の何れか１に記載のロボ
   ット制御装置において、 検出手段(40)は、制御対象部(12)の動作反力を検出する
   力センサであることを特徴とするロボット制御装置。
5. 【請求項５】 請求項２乃至４の何れか１に記載のロボ
   ット制御装置において、 変更手段(56)は、正弦波状の変化量の正弦波形を変更さ
   せる波形変更信号を出力することを特徴とするロボット
   制御装置。
6. 【請求項６】 請求項２乃至４の何れか１に記載のロボ
   ット制御装置において、 変更手段(56)は、フィードバック手段(55)の修正量を変
   更させる修正量変更信号を出力することを特徴とするロ
   ボット制御装置。