JPH0721733B2

JPH0721733B2 - 物体移動装置の制御方式

Info

Publication number: JPH0721733B2
Application number: JP60104860A
Authority: JP
Inventors: 和雄浅川; 信彦恩田; 徹鎌田; 正秋田
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1985-05-16
Filing date: 1985-05-16
Publication date: 1995-03-08
Anticipated expiration: 2010-03-08
Also published as: JPS61262817A

Description

【発明の詳細な説明】〔目次〕概要産業上の利用分野従来の技術発明が解決しようとする問題点問題点を解決するための手段（第１図）作用実施例（ａ）−実施例の構成の説明（第２図，第３図）（ｂ）−実施例の外力制御モードの説明（第４図，第５
図，第６図）（ｃ）−実施例の指令制御モードの説明（第７図，第８
図，第９図）（ｄ）他の実施例の説明（第10図）（ｅ）別の実施例の説明発明の効果〔概要〕物体を移動する移動手段を制御手段が速度制御して物体
を移動する物体移動装置において、制御手段が物体に加
わる力を検出する外力検出手段による力指令と速度指令
との合成を合成速度指令とするように系を構成し、且つ
力指令を合成速度指令として与える外力制御モードを備
えることにより、外力に追従した移動を行なうようにし
たものである。

〔産業上の利用分野〕

本発明は、ロボツト等の物体移動装置において、ハンド
等の物体に加わる外力に従つて物体を移動することので
きる制御方式に関する。

物体を移動する物体移動装置は種々の分野で広く利用さ
れている。

例えば、ロボツトにおいてはハンドを所望の位置に移動
し、ハンドによる種々の作業を可能とする。

一般に係るロボツト等の物体移動装置では、予じめ定め
られた移動指令によつて移動制御を行なうようにしてい
るが、近年ロボツトの高知能化への要求に従い種々の制
御方式が提案されている。

〔従来の技術〕

係る制御方式において、ロボツトに力覚を付与し、外力
を検出して移動を行なうことが知られている。

例えば、特許出願公開昭56−85106号公報（特許出願昭5
4−161557号）には、ロボツトの教示作業を行なうた
め、ロボツトの先端に力検出器を設け、この力検出器か
ら発生する信号に基づいてロボツトの移動方向を演算
し、この演算された指令値に基づいて、ロボツトの各腕
を駆動する駆動装置を駆動せしめることにより、教示を
行なう操作者の労力の負担を軽減せしめるようにしたも
のが提案されている。

係る提案は外力によつて移動を行なうものの一種であ
り、ダイレクトテイーチ方式として知られたものであ
る。

〔発明が解決しようとする問題点〕

しかしながら、係る従来の提案では、ハンドの代りに力
検出器を設け、操作者が力検出器を持つて外力を付与す
ることにより移動を行なうものであるから、ハンドを付
けたまま外力によつて移動させることができない。この
ため、例えば、教示においては、実際にはバンドが動作
するための教示であるのに、ハンドを取り外すため、ハ
ンドの位置を想定した教示しかできず、正確な教示がで
きないという問題があつた。

又、ハンドに物品を保持せしめて手動による外力によつ
て物品を移動させる場合には、即ち運搬ロボツトとして
用いるには、ハンドが取り外されるため、不可能である
という問題もあつた。更に、通常の移動制御系と異なる
力制御系を構成する必要があるため、制御系が複雑とな
り、高価格化が避けられないという問題もあつた。

本発明は、物体（ハンド）を取外すことなく且つ移動制
御系を変更することなしに外力によつて物体を移動する
ことのできる物体移動装置の制御方式を提供することを
目的とする。

〔問題点を解決するための手段〕

第１図は本発明の原理説明図である。

第１図（Ａ）中、５は物体（ハンドと称す）であり、例
えば物品を吸着する吸着ハンドであり、３は力センサ
（力検出手段）であり、物体に加わる外力を検出するも
の、MTは移動手段であり、モータで構成され、力センサ
３及びハンド５を移動させるためのもの、８は制御部
（制御手段）であり、速度指令VCと力センサ３の力計測
値Ｆγとの合成をとる合成部85を有し、合成した速度指
令Ｖ′Ｃ（＝VC−Ｆγ）で移動手段MTを制御するもので
ある。

従つて、本発明では、基本的には速度制御系として速度
指令VCと力計測値Ｆγの合成を速度指令とする系が組ま
れている。

〔作用〕

本発明では、前述の如く制御系が組まれているので、通
常の移動指令制御モードにおいては、速度指令VCが発せ
られる。この場合、ハンド５に外力が加わつておらず、
従つて、力計測値Ｆγが零なら、速度指令VCが合成速度
指令Ｖ′Ｃとなり、速度指令VCで移動手段が速度制御さ
れる。又、ハンド５に外力が加わり、力センサ３の出力
である力計測値Ｆγが零でなければ速度指令VCと力計測
値Ｆγとの合成（差）が合成速度指令Ｖ′Ｃとなつて外
力適応制御が実行される。

従つて、ハンド５の移動中に何等かの物体に触れると自
律的に外力適応制御に移行し、即ち速度指令VCが力計測
値Ｆγ分減少し、減速し、外部からの拘束力に応じた制
御が実行される。

一方、速度指令VCを零とすると、合成速度指令Ｖ′Ｃは
力計測値Ｆγとなり、外力制御モードが実行できる。従
つて、手HDによつてハンド５に触れ、外力を与えると、
力センサ３からの合成速度指令Ｖ′Ｃ＝（−Ｆγ）によ
つて、外力が零となる方向に移動手段MTが駆動され、ハ
ンド５は移動する。

従つて、第１図（Ｂ）の如く位置P₀にあるハンド５を手
HDで持つて矢印方向に引つぱつて外力を付与すると、第
１図（Ｃ）の如くの力計測値Ｆγ、即ち合成指令速度
Ｖ′Ｃが出力され、これによつて外力を零とする方向、
即ち外力が与えられた矢印方向に移動手段MTがハンド５
を移動する。位置P₁で手HDを停止し、外力を解除する
と、力計測値Ｆγも第１図（Ｃ）の如く落ち、従つて合
成指令速度Ｖ′Ｃも落ち、ハンド５は停止する。従つて
速度制御系を変更せずに外力による物体の移動ができ
る。

〔実施例〕

（ａ）−実施例の構成の説明第２図は、本発明の一実施例構成図であり、直交型ロボ
ツトを例にしたものである。

図中、第１図で示したものと同一のものは同一の記号で
示してあり、1a,1bはＸ軸モジユールであり、ロボツト
の２つのＸ軸位置決め機構を構成し、各々Ｘ軸モータ10
a,10bにより搬送パレツト11a,11bをＸ軸方向へ搬送位置
決めするもの、２は門型ロボツトであり、Ｘ軸モジユー
ル1a,1bの両側に設けられた一対の支持ベース20,21と、
Ｙ軸方向に移動するＺ軸ブロツク22と、Ｚ軸方向に移動
するＺ軸可動部（アーム）23と、Ｚ軸ブロツク22を送
り、ボールネジ24aを回転させガイド25a,25bに沿つてＹ
軸方向に駆動するＹ軸モータ24と、Ｚ軸ブロツク22に設
けられ、Ｚ軸アーム23を図示しないボールネジ送り機構
を介しＺ軸方向に駆動するＺ軸モータ26とを有してい
る。３は前述の力センサであり、板バネ機構と歪ゲージ
とから成り、外力による板バネの変位を歪ゲージで電気
信号に変換するものであり、X,Y,Z,γ軸の４自由度力セ
ンサで構成されるもの、４はγ軸モータであり、Ｚ軸ア
ーム23に支持され、力センサ３及びハンド５をγ軸を中
心に回転させるもの、５は真空吸着ハンドであり、後述
する第５図に示す如く筒状本体の先端に吸着面が設けら
れるとともに、吸気ポンプに接続された吸気チユーブを
有するもの、６は治具であり、パレツト11a上で円板
（磁気デイスク板）９を固定するもの、７はベースであ
り、パレツト11bに搭載され、円板９が取付けられるも
のである。

80は操作パネルであり、オペレータが操作してコマン
ド，データを入力するもの、81はメモリであり、教示デ
ータ等を格納するもの、82はプロセツサ（以下CPUと称
す）であり、マイクロプロセツサ等で構成され、プレイ
バツク時にメモリ81の内容を読出して各部へ指令を与え
るもの,83はサーボ制御部であり、Ｘ軸モジユール1a,1b
のＸ軸モータ10a,10b及びＹ軸モータ24、Ｚ軸モータ26
及びγ軸モータ４を位置，速度制御するため、CPU82か
らの指令位置CX₂、CX₁、CY,CZ,Cγが入力され、後述す
るハンド位置検出回路からの現在位置PX₂、PX₁、PY,PZ,
Pγがフイードバツクされ、更に合成部85から指示速度V
CX〜VCγが入力され、これらによつてサーボ制御するも
の、84はパワーアンプであり、入力を増幅して、X,Y,Z,
γ軸モータ10a,10b,24,26,4に電流を供給するもの、85
は前述の合成部であり、第３図にて後述する如く、各軸
の指令速度VX₁〜Ｖγと後述する力制御部の力制御出力P
FX〜PFγとの差をとり、サーボ制御部83へ与えるもの、
86はテイーチングボツクスであり、教示を行うために操
作されるものであり、ダイレクトテイーチを指示するキ
ー86aと、Ｚ軸上昇指示を与えるＺ軸キー86bと、教示終
了を指示するキー86Cとを有するもの、87は力制御部で
あり、第３図にて後述する様に力センサ３の検出出力FX
〜Ｆγを受け、これをデジタル値FX〜Ｆγに変換すると
ともに不感帯を設定して制御出力PFX〜PFγを出力する
もの、88はハンド位置検出部であり、各軸のモータ10a,
10b,24,26,4に設けられたロータリーエンコーダの出力
から各軸の現在位置PX₁，PX₂,PY,PZ,Pγを求め、ハンド
５の現在位置を得るもの、89はバスであり、CPU82とメ
モリ81,操作パネル80,テイーチングボツクス86,サーボ
制御部83,合成部85,力制御部87及びハンド位置検出回路
88とを接続し、データ，コマンドのやりとりを行なうも
のである。

第３図は力制御部87,合成部85,サーボ制御部83及びパワ
ーアンプ84の詳細回路である。

図中、第２図で示したものと同一のものは同一の記号で
示してあり、800〜803は各軸の合成回路であり、力制御
部87からの力制御指令PFX〜PFγとCPU82からの速度指令
VX〜VZとの差を出力するもの、804〜807は不感帯部であ
り、CPU82からの不感帯幅WX〜Ｗγで設定され、力計測
値FX〜Ｆγに対し不感帯を与えるもの、808はスイツチ
であり、力計測値FX〜Ｆγの入力許可，不許可をするも
のであり、力フイードバツクオン／オフをCPU82の制御
により行うもの、809はアナログ／デジタルコンバータ
（A/Dコンバータと称す）であり、力センサ３からのア
ナログの力計測値FX,FY,FZ,Fγをデジタル値に変換し
て、スイツチ808に出力するもの、83a〜83dは各々サー
ボ回路であり、位置検出器88からの現在位置と指令位置
CX,CY,Cγとの差に基いて位置制御し、且つ指令速度
Ｖ′Ｘ〜Ｖ′γと実速度との差に基いて速度制御するも
の、84a〜84dはパワーアンプであり、各々サーボ回路83
a〜83dの出力に基いて各軸のモータに駆動電流を供給す
るものである。

従つて、CPU82によつてスイツチ部808がオフの時には、
力センサ３の出力（即ちA/Dコンバータ809の出力）は不
感帯部804,805,806,807へ入力されず、力フイードバツ
クオフとなり、CPU82からの指令位置CX,CY,CZ,Cγ及び
指令速度VX,VY,VZ,Vγがそのまま各軸のサーボ回路83a
〜83dに入力され、位置、速度制御される。

一方、CPU82によつてスイツチ部808がオンの時には、力
フイードバツクオンとなり、力センサ３の出力は不感帯
部804〜807を介し制御出力PFX,PFY,PFZ,PFγとなつて合
成部800〜803に入力し、指令速度VX,VY,VZ,Vγとの合成
出力Ｖ′X,V′Y,V′Z,V′γがサーボ回路83a〜83dに速
度指令として与えられる。

尚、Ｘ軸はX1軸とX2軸の２つがあるが、１つのＸ軸サー
ボ回路83a,アンプ84aで示してあり実際には２つある。

（ｂ）−実施例構成による外力制御（教示）モードの説
明。

第４図は、外力制御モードの一例としての教示モードの
処理フロー図、第５図はその動作説明図、第６図はメモ
リの内部構成図である。

第５図に示すパレツト11a上の治具６にセツトされた円
板９を吸着ハンド５の吸着面50で吸着して取り出し、パ
レツト11b上のベース７に設けられたスピンドル12には
め込む作業を例に以下説明する。

オペレータは、テイーチングボツクス86のキー86aを
操作し、バス89を介しCPU82に教示モードを通知する。C
PU82はこれによつてダイレクトテイーチが指示されたと
判定し、バス89を介し力制御部87のスイツチ808をオン
し、力フイードバツクオンとする。更にCPU82は、バス8
9を介し力制御部87の各不感帯部804〜807に不感帯幅WX,
WY,WZ,Wγを設定する。

オペレータは、ハンド５を手に持つて目標位置の頭上
（Ｂ点）方向にハンド５をひつぱる（又は押す）。ハン
ド５に手によつて外力が付与されると、力センサ３より
外力付与方向の外力が検出される。例えば、第５図の場
合＋Ｙ方向の外力が付与され、力センサ３よりＹ軸の力
計測値FYが出力される。この力計測値FYはA/Dコンバー
タ809でデジタル値に変換された後、不感帯部805に入力
される。

各不感帯部804〜807は力計測値FX〜Ｆγに対し非線形要
素を通した信号のフイードバツクを行なうものであり、
入力の絶対値が不感帯幅設定値Ｗより小さい時は出力を
ゼロとする。一方、入力の絶対値が不感帯幅設定値Ｗよ
り大きい時は、入力が正値の時は、（入力−不感帯幅設定値）を出力と
し、入力が負値の時は、（入力＋不感帯幅設定値）を出力と
する。

この不感帯部805〜807の存在によつてハンド５が空中に
浮いた状態で、その場所に止めておきたい時に力センサ
３の力計測値にオフセツト変動があると、それにより、
ハンド５の位置がドリフトすることがあるが、不感帯幅
より小さなオフセツト変動に対しては、不感になり、ド
リフト現象が生じない。

又、ハンド５をオペレータが手で導く時にオペレータへ
の応答として適当な抵抗力（不感帯幅に対応）を発生す
ることができる。

従つて、力計測値FYは不感帯部805で前述の不感帯処理
が施され、力指令PFYとして合成回路801に入力する。こ
こで、CPU82からの位置指令CX,CY,CZ,Cγ及び速度指令V
X,VY,VZ,Vγは零であるから、合成速度指令Ｖ′Ｙ′はP
FYそのものでありＹ軸サーボ回路83bに速度指令として
入力し、パワーアンプ84bを介しＹ軸モータ24を駆動す
る。従つて、ハンド５は外力が付与された方向のＢ点に
向つてハンド５を持つ手の動きに追従移動する。オペレ
ータはハンド５がＢ点に到達すると、手をハンド５から
離す。この時も不感帯部804〜807によつてドリフト現象
が防止され、ハンド５はＢ点に停止する。

次にオペレータはハンド５を手に持つて、手をＣ点方
向に下降する。ステツプと同様に、力センサ３からは
外力●付与方向のＺ軸方向の力計測値FZが出力され、A/
Dコンバータ809、スイツチ808及び不感帯部806を介し力
指令PFZが発生し、同様に合成速度指令VZ（＝−PFZ）が
Ｚ軸サーボ回路83cに入力し、パワーアンプ84cを介しＺ
軸モータ26を駆動する。従つて、ハンド５は外力の付与
された方向のＣ点に向つて、ハンド５を持つ手の動きに
追従移動する。更にオペレータは治具６にハンド５をは
め合わせる。この時、X,Y方向にずれがあれば、即ちＢ
点が治具６の中心に一致していないとはめ合いができな
いから、オペレータはハンド５にＸ又はＹ方向に外力を
与え、力センサ３よりＸ軸又はＹ軸の力計測値FX,FYを
発生し、同様にＸ軸モータ10a又はＹ軸モータ24を駆動
してハンド５を手の動きに追従移動させ、はめ合いを行
なう。これによつてハンド５は正確に治具６（即ち、円
板９）の中心に位置することになり、X₂軸,Y軸の要素が
確定する。

オペレータはハンド５から手を離し、テイーチングボ
ツクス86のＺ軸上昇キー86bを押下する。これによつてC
PU82はＺ軸速度指令VZを発し、キー86bの押下中VZをバ
ス89を介し合成回路802に与え、合成速度指令Ｖ′Ｚ＝V
ZをＺ軸サーボ回路83cに与え、パワーアンプ84cを介し
Ｚ軸モータ26を駆動し、ハンド５をＺ軸方向に上昇せし
める。ハンド５が治具６から外に出た時も、前述の不感
帯部804〜807によつてX₂軸,Y軸はその位置が保たれる。
即ち不感帯なる非線形演算を施すことにより人工的にク
ーロンまさつ特性を作り出しているため、一定以上の外
力が付与されないと動かされない。ハンド５が治具６か
ら完全に出た所の所望の位置でキー86bの押下を止める
と、CPU82はＺ軸速度指令VZを零とするので、ハンド５
の上昇は停止する。

次に、オペレータはテイーチングボツクス86の終了キ
ー86cを押下すると、CPU82は教示モードを終了し、操作
パネル80のキーボードからのコマンド受け付け状態とな
る。オペレータがこのキーボードからコマンドとしてPO
INT指示（POINT DISK）を入力する。

このコマンドによりCPU82は位置検出器88の各軸の値
Ｘ（X₁，X₂）,Y,Z,γをバス89を介し読みとり、メモリ8
1に教示データとして格納する。

メモリ81は第６図に示す如く作業テーブル81aと、座標
テーブル81bとを有し、作業テーブル81aには、作業シー
ケンス順にマクロ命令が予じめ記述されている。例えば
シーケンス“100"の「PICK DISK」は、ある点（後述）
に移動し、指定速度で降下し、円板（DISK）９を吸着
（PICK）し、ある点に戻るというコマンドであり、シー
ケンス“100"の「PLACE SPINDLE」は、ある点に移動
し、指定速度で降下し、吸着したものをスピンドル（SP
INDLE）に置いて、ある点に戻れというコマンドであ
る。このようなマクロ命令「PICK」「PLACE」等を設け
ることにより作業指示が容易となる。一方、座標テーブ
ル81bには前述のマクロ命令の目的語である「DISK」，
「SPINDLE」の前述のある点の位置座標を格納してあ
り、前述のCPU82で読取つた各軸の位置が格納される。

従つて座標テーブル81bには、「POINT DISK」がステツ
プで指示されると、「DISK」の欄に各軸の位置が格納
され、この位置は目標物品（円板９）の真上の座標（例
えば第５図のＧ点）を示すことになる。

以下、円板９をスピンドル12にはめ合わせるための教示
も同様にして行なわれる。

ここで、円板９が複数枚治具６に積層されていても「DI
SK」の欄の１つの座標値で次の（ｃ）で述べる如く取り
出しが可能となる。即ち、各円板９の深さ方向の教示を
行なう必要がなく外力適応制御によつて取り出しが円滑
に行なわれる。

（ｃ）一実施例構成による指令制御モードの説明。

第７図は移動指令モードとしての円板吸着動作（PICK D
ISK）の処理フロー図、第８図は移動指令モードの説明
図、第９図は移動指令モードの動作説明図である。

操作パネル80よりSTARTコマンドが入力されると、バ
ス89を介しCPU82に動作開始命令が与えられる。

これによつて、CPU82はメモリ81の作業テーブル81aの命
令文を解読し、「PICK DISK」であると、吸着取出し（P
ICK）であると判定し、取出し処理を開始する。

次に、CPU82はメモリ81の座標テーブル81aの「DISK」欄
を読み出し、前述のＧ点（X₂,Y,Z）の座標を得、これに
よる位置指令CX₂,CY,CZをバス89を介し、サーボ制御部8
3へ与え且つ合成部85にX,Y,Z速度指令VX₂,VY,VZを与え
る。

これによつて、サーボ制御部83よりパワーアンプ84を介
し駆動電流SX₁,SY,SZがＸ軸モータ10a,Y軸モータ24,Z軸
モータ26に供給される。

従つて、Ｘ軸（X₂軸）モジユール1aのＸ軸モータ10a、
Ｙ軸モータ24、Ｚ軸モータ26が駆動されて、真空吸着ハ
ンド５はＸ軸モジユール1a上のパレツト11aの治具６の
円板９上の指示されたＧ点（第５図）に位置決めされ
る。

次に、CPU82はハンド位置検出回路88の各軸の現在位
置PX₂,PY,PZからの所定のハンド位置に到達したか否か
を調べ、そして所定の位置に停止した後の所定の時間経
過後（0.5秒程度）、力センサ３の振動停止とみなし、
力フイードバツクをオンとする。即ち、力制御部87のス
イツチ808をオンとし、力制御部87の出力PFZを合成部85
へ入力可能とする。

CPU82は、バス89を介し速度指令値VZを合成部85へ与
える。前述の如く、円板９への接触前は力指令PFZ＝０
であるから、サーボ回路83cへは指令速度として出力さ
れ、Ｚ軸モード26を速度制御する。従つて、吸着ハンド
５は円板９に向つて指令速度V₁で下降する。

一方、CPU82はバス89を介し力制御部87のA/Dコンバー
タ809の力計測値FZを監視し、FZが所定の値Ｍとなる
と、吸着ハンド５が円板９に接触したと判定する。

これとともに前述の如く外力適応制御に自律的に移行す
る。これを第８図によつて説明する。

ハンド５が円板９に接近中では力センサ３の力計測値FZ
は零であるから、Ｖ′Ｚ＝VZであり、Ｚ軸モータ26はサ
ーボ回路83cによつてパワーアンプ84cを介し速度制御さ
れ、ハンド５を下降せしめる。

一方、ハンド５が円板９に位置P₁（時刻t₁）で接触する
と、力センサ３が板バネで構成されているからたわみ、
このたわみを検出する歪ゲージ30より力計測値FZが発生
するが、不感帯幅WZに対応する押し付け力（力計測値F
Z）になるまで指令VZで与えられた速度で移動を続け、
時刻t₂でFZ＞WZとなることによつて、力指令PFZが発生
する。従つて、合成回路802の出力Ｖ′Ｚは（VZ−PFZ）
となり、合成速度指令Ｖ′Ｚは、速度指令VZから力指令
PFZが差し引かれた形となり、見かけ上速度指令値を小
さくした形となり、ハンド５は減速し始める。力センサ
３の平行板バネがモータ26の回転で更にたわみ、力計測
値FZは上昇し、ハンド５は更に減速する。

最終的な平衡状態では、時刻t₀で位置P₀でＺ軸モータ26
が停止し、実速度値が零となる。この時入力指令VZと力
指令PFZが等しく、力指令PFZは力センサ３の変形によつ
て物体９への押し付け力F₀となるから、入力指令VZは力
指令として働く。従つて、入力指令VZの大きさが押し付
け力を制御することになる。

この時の押し付け力は、入力指令VZによる力指令値と不
感帯幅Ｗに対応した力との和になり、一方、接近中の速
度は入力指令VCだけに依存するので、移動速度と押し付
け力を独立に制御できる。

入力指令VZは接近時の速度指令値と押し付け力発生時の
力指令値の両方を兼ているため、速度計測値VZと力計測
値FZの出力レベルの相対的な大きさによつては、１つの
入力VZで最適な移動速度と、最適な押し付け力の両方を
同時に満たせないおそれがある。

このため、力計測値FZのフイードバツク量を可変にする
ための入力を設け、即ち、不感帯幅WZによつて、速度指
令VZと不感帯幅WZを独立に設定し、最適な速度指令値
と、力指令値とを１つの入力指令で得るようにしてい
る。又、力計測値FZのフイードバツクゲインを固定でき
ることから、閉ループ制御系としてのループ利得を一定
に保つたまま（つまり制御系としての安定性を保証しつ
つ）押し付け力を広い範囲で可変できること、及びハン
ド５が空中に浮いた状態で（つまりハンド５が他の物体
を押していない時）、入力をゼロとし、その場所に止め
ておきたい時、力センサ３の力計測値にオフセツト変動
があると、それにより、ハンド５の位置がドリフトする
ことがあるが、この実施例では、不感帯幅より小さなオ
フセツト変動に対しては、不感になり、ドリフト現象は
なくなる。

従つて、力センサ３のたわみ（変形）により接触時のシ
ヨツクを吸収しつつ、モータ26の回転速度を連続的に落
としていき、速度が零となつた所で、適切な押し付け力
を発生するという理想的な形とすることができる。

このようにして接近、接触、押し付けの３過程が連続的
に円滑に且つ自律的に行なわれる。

即ち、ロボツトによる物体の取り出し作業等において
は、ロボツトのハンドが物体に接近し、接触した後、一
定力で物体を押し付けて、把持を行つており、同様に物
体の取付け作業等においても、相手物体に対し物体を把
持したハンドが相手物体に接近し、接触した後、相手物
体に把持した物体を一定力で押し付け、はめ込み、取付
けを行う。

本発明では、移動体であるハンドの前述の接近、接触、
一定力発生という３つの過程の制御を連続的に、且つ自
律的に行なうことができる。

即ち接触の前後で連続的にモータの回転速度が落るため
接触時の衝撃を小さく抑えることができ、力センサ３の
リニアな力出力値を使つて速度制御モードから力制御モ
ードへ、タイミングよく、かつなめらかに切り替えてい
くことができる。また押し付け力制御時は速度計測値
（実速度）のフイードバツクは状態変数フイードバツク
の、いわゆるダンピング項として働らくため、制御系と
しても、きわめて安定な形である。更に第５図のよう
に、磁気デイスク９とスペーサー90を積み重ねたものを
順に取り上げ、他の場所に移す時のように、ハンド５が
磁気デイスク９、又はスペーサ−90を取る時の深さが、
変化していく場合でも、希望のハンド下降速度と、希望
の押し付け力が常に、かつ自動的に得られるため、深さ
方向の距離のテイーチングが不用になつて、ロボツトに
作業を教示する人間の負担が軽くなり、ロボツトの知能
化が一歩進むことになる。

又、この時、第９図に示す如くＸ軸方向に位置ずれがあ
り、ハンド５が円板９に接近中に治具６に接触して、は
め合いが円滑に行なわれない場合にも、治具６との接触
により力センサ３からＸ軸の力計測値FXが第８図（ｃ）
の如く発生し、FX＞WXとなるとこれによつて力指令PFX
が出力され、合成回路800を介し合成速度指令Ｖ′Ｘ
（＝−PFX）が発生し、Ｘ軸サーボ回路83a及びパワーア
ンプ84aを介しＸ軸モータ10aが駆動され、この場合、パ
レツト11a、即ち治具６が図の矢印Ｘ方向に動き、ハン
ド５と治具６のＸ軸方向の相対位置誤差が修正される。
このことはＹ軸に対しても同様であり、結局最大抗力が
不感帯幅より小さい範囲では治具６に沿いながら、力セ
ンサ３のたわみによつてハンド５と治具６の相対位置誤
差が吸収され、一方、不感帯幅を越えると、前述の如く
モータの駆動によつて相対位置誤差の修正が行なわれ、
治具６の中心（円板９の中心）とハンド５の中心が自動
的に合わされていき、挿入が実行される。この不感帯幅
を変えることは、電気的にみかけ上まさつ力を変えてい
ることになる。

更に、CPU82はバス89を介し、吸気チユーブの負圧を
検出する図示しない圧力センサの出力を監視し、吸着ハ
ンド５が円板９を吸着したかを検出する。

CPU82は、吸着ハンド５が円板９を吸着したと判定す
ると速度指令値VZを零とし、次に前述のスイツチ808を
オフとして力フイードバツクをオフとする。

更にCPU82は、吸着ハンド５を上昇すべく、前述の座標
テーブル81bの「DISK」の欄の座標を読み出し、ステツ
プと同様にしてＧ点へ戻す。これによつて、円板９の
取出しが行なわれる。次にこの取り出した円板９をスピ
ンドル12にはめ合せるには前述と同様に作業テーブル81
aの次のシーケンス「PLACE SPINDLE」を読出し、これに
よつて座標テーブル81bの「SPINDLE」の欄の座標を読出
し、ステツプ，，と同様にしてＺ方向に下降せし
め、ベース７への接近、接触、押し付けを行い、吸着を
解除して円板９のベース７への取付けを行う。

このようにして、移動指令モードでは、物体への接近、
接触、押し付けが自律的に実行され、即ち外力適応制御
へ自律的に移行し、更に教示データによる位置に対し位
置ずれがあつても（例えば治具６の位置が微妙にずれて
いても）、これを修正することができる。

（ｄ）他の実施例の説明第10図はCPU82の処理説明図である。この例では、合成
部85及び力制御部87の機能をCPU82のプログラムの実行
によつて行なうものである。

CPU82はメインルーチンにおいて、教示データのコマン
ドを解析し、これを実行して各軸の位置指令、速度指
令、力指令を作成し、バス89を介しサーボ制御部83へ出
力し、又位置検出部88からの現在位置Ｐγ〜PX₂によつ
て各軸の位置及び力センサ３の力計測値Ｆγ〜FXをD/A
コンバータを介し監視する。

そして、力フイードバツクオフモードでは、スイツチが
矢印点線の如く接続され、指令位置と指令速度をそのま
まサーボ制御部83へ与えて、各軸サーボ回路83a〜83dを
介し各軸を指令位置に位置決めする。一方、力フイード
バツクモードオンにおいては、スイツチが矢印点線の如
く接続され、所定周期で力フイードバツク制御の割込み
処理ルーチンを実行する。即ち、力センサ３の力計測値
Ｆγ〜FXをオフセツト補正し、さらに不感帯処理して、
帰還利得αを掛け制御出力PFX〜PFZを得、これを指令速
度又は指令力から差し引いたものを指令速度Ｖ′γ〜
Ｖ′Ｘ₂としてバス89を介しサーボ制御部83へ与える。

この場合、帰還利得αは、第８図（Ａ）の減速の傾きを
制御し、前述の不感帯と同様に、指令速度と押し付け力
を一つの入力で制御することができる。従つて、力指令
PFは＝α・（Ｆ−Ｗ）となる。

（ｅ）別の実施例の説明前述のサーボ回路83として、本発明者等の提案による関
数発生部と閉ループ制御部とから成るものを利用する
と、一層、安定なサーボ系が実現できる。このサーボ回
路は、例えば雑誌「日経エレクトロニクス1981・９・28
号」（日系マグロウヒル社発行）等において周知である
ので詳述しない。

又、ロボツトをＸ軸が分割された直交型のもので説明し
たが、他の直交型ロボツトにも適用でき、更に多関節型
等のロボツトであつてもよい。

更に動作例として磁気デイスク装置の組立てを例に説明
したが、これに限られず、他の動作であつてもよく、し
かも教示を例に説明したが、ハンド５によつて物品を把
持せしめ、これを外力によつて所望の位置に搬送するも
のや、ハンド５が作業に都合の悪い位置にある時に、こ
れを別の位置に移動させるもの等であつてもよい。

以上本発明を実施例により説明したが、本発明は本発明
の主旨に従い種々の変形が可能であり、本発明からこれ
らを排除するものではない。

〔発明の効果〕

以上説明した様に、本発明によれば、次の効果を奏す
る。

力指令と速度指令との合成信号によって駆動手段を制
御する系を構成し、力指令と速度指令との合成によって
制御する通常制御モードと、力指令を合成速度指令とす
る外力制御モードを備えるため、外力のみに追従する制
御と外力適応制御とを制御系を変えずに実行できる。

力検出手段の出力に不感帯を付与した力指令を作成す
るので、通常制御モード時は、力検出手段のオフセット
により、物体位置がドリフトするのを防止できる。

又、不感帯を付与したため、通常制御モードで移動中
に生ずる振動が力検出手段で検出され制御手段にフィー
ドバックされ、振動が増大するのを防止できる。

不感帯を付与したため、物体をオペレータが手で持っ
ていく教示時に、オペレータへの応答として適度の抵抗
力を付与できる。

不感帯を付与したため、教示時に、手で触れただけで
は動作せず、力を加えた時のみ移動するので、円滑に教
示が可能となる。

【図面の簡単な説明】

第１図は本発明の原理説明図、第２図は本発明の一実施例構成図、第３図は第２図構成の要部回路図、第４図は本発明による外力制御モードの処理フロー図、第５図は第２図構成の動作説明図、第６図は第２図構成のメモリの内部構成図、第７図は本発明による移動指令モードの処理フロー図、第８図は第７図移動指令モードの説明図、第９図は第７図移動指令モードの動作説明図、第10図は本発明の他の実施例説明図である。図中、３……力検出手段、５……物体（ハンド）、８…
…制御手段、MT……移動手段。

───────────────────────────────────────────────────── フロントページの続き (72)発明者秋田正神奈川県川崎市中原区上小田中1015番地富士通株式会社内 (56)参考文献特開昭60−3010（ＪＰ，Ａ) 実開昭58−169986（ＪＰ，Ｕ)

Claims

【特許請求の範囲】

【請求項１】物体を移動する移動手段と、該物体に加わる外力を検出する力検出手段と、速度指令から該力検出手段の検出出力に不感帯を付与し
た力指令を減じた合成速度指令で該移動手段を移動制御
する制御手段を有し、前記制御手段は、前記速度指令と前記力指令との合成速
度指令で制御する通常制御モードと、前記力指令を前記合成速度指令として制御する外力制御
モードとを備え、対象物体への移動時は、該通常制御モードを行い、該物
体を手動操作する教示時は、該外力制御モードを行うこ
とを特徴とする物体移動装置の制御方式。