JPS61208513A - 移動体制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔目次〕 概要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段（第１図）作用 実施例 （ａｌ−実施例の説明（第２図、第３図）（ｂ）他の実
施例の説明（第４図、第５図）（Ｃｌロボットへの通用
例の説明 （第６図、第７図、第８図、第９図） 発明の効果 〔概要〕 物体に対し移動体を駆動手段の駆動によって接近、接触
、押し付けを行なわしめる移動体制御装置であって、移
動体に加わる外力の力検出手段と、駆動手段を速度制御
するサーボ手段と、指令値と検出外力とを合成した信号
をサーボ手段に供給する制御手段を設けることによって
、接近、接触、押し付けを連続的に行うようにしたもの
である。

〔産業上の利用分野〕

本発明は、ロボットのハンド等の移動体に対し、物体に
接近、接触後、一定力で物体を押し付ける作業を行わせ
るための移動体制御装置に関し、特に接近、接触、押し
付は力発生の３過程を連続的に進行することの可能な移
動体制御装置に関する。

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

このため、移動体であるハンドを前述の接近、接触、一
定力発生という３つの過程で制御する必要があり、一般
に速度（又はトルク）制御と力制御とを併用した制御系
が用いられる。

〔従来の技術〕 例えば、第１０図に示す真空吸着ハンド５によって磁気
ディスク（以下円板と称す）９を吸着し、これを持ち上
げて運ぶ作業を行う場合には、ハンド５を駆動するＺ軸
モータ２６をハンド下降のための速度制御と、必要な押
し付は力を発生する力制御の２つの制御モードを切替え
て制御する必要がある。

このモード切替えは、ハンド５と円板９の接触を境界と
して切り替わることが理想的である。

このため、従来、力センサ３によってハンド５を支持し
、ハンド５に与えられる力を計測できるようにし、且つ
Ｚ軸モータ２６の速度（即ち、ハンド５の移動速度）を
検出する速度検出器２６ａが設けられ、一方、制御部と
しては、指令速度ＶＣと速度検出器２６ａの実速度Ｖｒ
との差をとる第１の差動器ＡＰＩと、指令力Ｆｃと力セ
ンサ３の力計測値Ｆｒとの差をとる第２の差動器ＡＰ２
とによって速度フィードバンク系とカフィードバック系
が構成され、これらの差動器ＡＰＩ、ＡＰ２の出力はア
ナログスイッチＳＷによって切替えてアンプＡＭＰを介
してＺ軸モータ２６に供給するように構成されていた。

そして、この切替えのため、ハンド５の先端に近接セン
サＮＳを設け、ハンド５と円板９との接近を検知して、
これによって接触前後でアナログスイッチＳＷを動作し
て速度制御系から力制御系に切り替えを行っていた。

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

しかしながら、従来の構成では、速度制御系から力制御
系へループ切り替えするため、動作が不連続となり、接
触直後に急激な力が付与され、円板９を傷つけるおそれ
があるという問題があった。

又、近接センサＮＳの感度の調節具合によっては、接触
の時点をうまくとらえることができず、接触前に切り替
わる時は高速性が損なわれ、逆に接触後に切り替わる時
は、円板９に余分の衝撃力を与えてしまうという問題も
生じていた。

本発明は、接近、接触、押し付けの３過程を連続的に円
滑にしかも自律的に実行できる移動体制御装置を提供す
ることを目的とする。

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

第１図は本発明の原理説明図であり、第１図（Ａ）はそ
の構成図、第１図（Ｂ）、（Ｃ）はその動作説明図であ
る。

第１図（Ａ）中、第１０図で示したものと同一のものは
同一の記号で示してあり、８は制御部であり、入力指令
値（速度指令又は力指令）Ｖｃと力計測値Ｆｒとを合成
する合成部８５と、合成部８５の出力Ｖｃ’と実速度Ｖ
ｒとの差によってＺ軸モータ２６を速度制御するサーボ
回路８６とを有している。

即ち、本発明においては、接近、接触、押し付けの３つ
の過程を通して、実速度Ｖｒと実側力Ｆｒとの両方が常
にフィードバックされている系を構成している。

〔作用〕 第１図（Ｂ）の移動量対速度特性図、第１図（Ｃ）の時
間対力持性図に示す如く、ハンド（移動体）５の円板（
物体）９への接近中は、ハンド５が円板９を押していな
いから、力計測値Ｆｒは零であり、実質的にフィードバ
ックされているのは、実速度Ｖｒのみであるため、入力
指令ＶＣは速度を指令していることになり、Ｚ軸モータ
２６はサーボ回路８６によって速度制御される。次にそ
の速度のまま、ハンド５が円板９に接触すると（時刻ｔ
１）、力センサ３が変形し、これに応じて力計測値Ｆｒ
が発生し、従って入力指令Ｖｃの値は力計測値Ｆｒに差
し引かれた形となり、みかけ上達度指令値を小さくした
形になって、ハンド５は減速し始める。

最終的な平衡状態では、時刻ｔｏで位置ＰｏでＺ軸モー
タ２６が停止し、実速度値が零となる。

この時入力指令Ｖｃと力計測値Ｆｒが等しく、力計測値
Ｆｒは力センサ３の変形によって物体９への押し付は力
Ｆｏとなるから、入力指令Ｖｃは力指令として働く。従
って、入力指令Ｖｃの大きさが押し付は力を制御するこ
とになる。

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

このようにして接近、接触、押し付けの３過程が連続的
に円滑に行われ、しかも近接センサを用いずに自律的に
行われる。

〔実施例〕

（８１一実施例の説明。

第２図は本発明の一実施例構成図である。

図中、第１図及び第１０図で示したものと同一のものは
同一の記号で示してあり、８２はプロセッサ（以下ＣＰ
Ｕと称す）であり、マイクロプロセッサで構成され、力
計測値Ｆｒに所定の利得αを利得手段８２ａで乗算し、
乗算出力α・Ｆ「を入力指令Ｖｃから合成手段８５で差
し引き指令出力Ｖｃ’を発するものであり、８６はサー
ボ回路であり、指令出力Ｖｃ’と実速度ＶｒＯ差をとる
もの、８６ａはパワーアンプであり、サーボ回路８６の
出力を増幅してＺ軸モータ２６に電流を供給するもので
ある。尚、力センサ３は平行板バネと歪ゲージ３０とで
構成されている。

第２図構成の動作を第３回動作説明図を用いて説明する
。

ハンド５が円板９に接近中では力センサ３の力計測値Ｆ
ｒは零であるから、Ｖｃ’＝Ｖｃであり、Ｚ軸モータ２
６はサーボ回路８６によってパワーアンプ８６ａを介し
速度制御され、ボールネジ２６ｂを回転させて、力セン
サ３を介しハンド５を下降せしめる。一方、ハツト５が
円板９に位置Ｐｌで接触すると、力センサ３が板ばねで
構成されているからたわみ、このたわみを検出する歪ゲ
ージ３０より力計測値Ｆｒが発生し、ＣＰＵ８２によっ
てＶｃ’は（Ｖｃ−α・Ｆｒ）となる。これによって出
力Ｖｃ’は減少し、モータ２６は減速し始める。この時
減速の傾きは利得αで制御され、平行板ばねがモータ２
６の回転で更にたわみ、力計測値Ｆｒは上昇する。最終
的には位置ＰＯで実速度Ｖｒが零となり、この時、ｖＣ
−α・Ｆｒであり、ＦＯの押し付は力が発生している。

この利得αを乗算する意味は次の如くである。

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

このため、力計測値Ｆｒのフィードバンク量を可変にす
るための入力を設け、即ち利得αを設定して、入力指令
Ｖｃと利得αを独立に設定し、最適な速度指令値と、力
指令値を１つの入力指令で得ることができるようにして
いる。

以上のように、本発明では、従来方法のように、近接セ
ンサ等のアナログスイッチを必要とせず、しかも、接触
の前後で連続的にモータの回転速度が落ちるため、接触
時の衝撃を小さく抑えることができ、ひずみゲージ３０
のリニアな力出力値を使って速度制御モードから力制御
モードへ、タイミングよく、かつなめらかに切り替えて
いくことができる。また、押し付は力制御時は速度計測
値（実速度）のフィードバンクは状態変数フィードバン
クの、いわゆるダンピング項として慟らくため、制御系
としても、きわめて安定な形である。

本発明によれは、第２図のように、磁気ディスク９とス
ペーサー９０を積み重ねたものを順に取り上げ、他の場
所に移す時のように、ハンド５が磁気ディスク９、又は
スペーサー９０を取る時の深さが、変化していく場合で
も、希望のハンド下降速度と、希望の押し付は力が常に
、かつ自動的に得られるため、深さ方向の距離のティー
チングが不用になって、ロボットに作業を教示する人間
の負担が軽くなり、ロボットの知能化が一歩進むことに
なる。

（ｂ）他の実施例の説明。

第４図は本発明の第２の実施例の構成図である。

図中、第２図で示したものと同一のものは同一の記号で
示してあり、８２ｂは不感帯部であり、セントされた不
感帯幅Ｗの非線形特性の入出力特性を示すものである。

この実施例では、不感帯部８２ｂによって希望の速度と
希望の押し付は力を得るようにしている。

即ち、不感帯部８２ｂは、力計測値Ｆｒに対し非線形要
素を通した信号のフィードバンクを実行するものであり
、ＣＰＵ８２の演算によって行われる。

この非線形要素の演算方法は次の通りである。

先づ、入力の絶対値が不感帯幅設定値Ｗより小さい時は
出力をゼロとする。一方、入力の絶対値が不感帯幅設定
値Ｗより大きい時は、 入力が正値の時は、（入力−不感帯幅設定値）を出力と
し、 入力が負値の時は、（入力＋不感帯幅設定値）を出力と
する。

次に、第４図実施例構成の動作について第５図を用いて
説明する。

第５図に示す如く位置Ｐ１接触しても不感帯幅Ｗに対応
する押し付は力になるまで指令Ｖｃで与えられた速度で
移動を続け、その後は、即ちＦｒ〉Ｗとなることによっ
て第３図と同様に減速して停止する。

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

この実施例では、第２図の実施例に比し、次の利点が付
加される。即ち、力計測値Ｆｒのフィードバンクゲイン
を固定できることから、閉ループ制御系としてのループ
利得を一定に保ったまま（つまり制御系としての安定性
を保証しつつ）押し付は力を広い範囲で可変できること
、及びハンド５が空中に浮いた状態で（つまりハンド５
が他の物体を押していない時）、入力Ｖｃをゼロとし、
その場所に止めておきたい時、第２図の実施例では、ひ
ずみゲージ３０の力計測値にオフセット変動があると、
それにより、ハンド５の位置がドリフトすることがある
が、この実施例では、不感帯幅より小さなオフセット変
動に対しては、不感になり、ドリフト現象はな（なる。

また、第４図において、入力Ｖｃをゼロとし、ハンド５
の筒の所を教示者が手でもって上下に導くと、力センサ
３のひずみゲージ３０がそれを感じて、ロボットのハン
ドの動きが、作業者の手の動きに追従する。いわゆるダ
イレクトティーチ（直接教示）が可能である。これは、
入力Ｖｃがゼロであるため、力計測値Ｆｒに不感帯の演
算を施した値がゼロになるようにモータ２６が制御され
ることによる。

この場合も、不感帯幅の設定により、教示者がハンド５
の筒部から手を離した時のドリフト現象を防止すること
、ハンド５の筒部を教示者が手で導く時教示者への応答
として適当な抵抗力（不感帯幅に対応）を発生させるこ
とができる。

別の実施例として、ＣＰＵ８２内で利得制御及び不感帯
制御の両方を行ってもよく、利得手段８２ａ又は不感帯
部８２ｂをＣＰＵ８２のプログラムの実行によらず、別
のハードウェアで構成してもよい。又、実速度を検出器
２６ａによって計測せず、モータ２６の状態からオブザ
ーバ等によって推定値として得るようにしてもよい。

＋Ｃｌロボットへの適用例の説明。

第６図は本発明の直交ロボットに適用した場合の構成図
、第７図は第８図構成のハンド及び力センサ構成図、第
８図は第６図構成の合成部８５、サーボ制御部８６及び
力制御部のブロック図である。

図中、第１図、第２図、第１０図で示したものと同一の
ものは同一の記号で示してあり、１ａｚ１ｂはＸ軸モジ
ュールであり、ロボットのＸ軸位置決め機構を構成し、
各Ｘ軸モータ１０ａ、１０ｂにより搬送パレットｌｌａ
、ＩｌｂをＸ軸方向へ搬送位置決めするもの、２は門型
ロボットであり、Ｘ軸モジュールｌａ、ｌｂの両側に設
けられた一対の支持ベース２０．２１と、Ｙ軸方向に移
動するＺ軸ブロック２２と、Ｘ軸方向に移動する２軸可
動部（アーム）２３と、Ｚ軸ブロック２２を送り、ポー
ルネジ２４ａを回転させガイド２５ａ、２５ｂに沿って
Ｙ軸方向に駆動するＹ軸モータ２４と、Ｚ軸ブロック２
２に設けられ、Ｚ軸可動部２３を図示しないボールネジ
送り機構を介しＸ軸方向に駆動するＺ軸モータ２６とを
有している。３は前述の力センサであり、第７図（Ａ）
に詳細を示ず如くＺ軸可動部２３に対し一対の板ばね３
ａ、３ｂが平行にねじ等で固定され、更に後述する支持
部材に固定され、且つ板ばね３ａ、３ｂの内面番こ各々
一対の歪ゲージ３０ａ、３０ｂ。

３１ａ、３１．　ｂが設けられているものであり、歪ゲ
ージ３０２〜３１ｂは第７図（Ｂ）に示す如くブリッジ
接続され、入力電圧Ｖｉｎに対し、出力電圧■Ｏｕｔを
得て、その値より平行板ばね３ａ、３ｂの吸着ハンド５
の軸方向の変位を検出するものであり、歪ゲージ３０ａ
、３０ｂは圧縮荷重を受ける側、３１ａ、３１ｂは引張
荷重を受ける側のものである。

４はハンド支持部材であり、吸着ハンド５を支持すると
ともに第７図（Ａ）の如く平行板ばね３ａ、３ｂが固定
されるもの、５は真空吸着ハンドであり、第７図（Ａ）
に示す如く筒状本体５０の先端に吸着面５１が設けられ
るとともに、吸気ポンプに接続された吸気チューブ５２
を有するもの、６は治具であり、パレットｌｌａ上で円
板９を固定するもの、７はベースであり、パレットｌｌ
ｂに搭載され、円板９が取付けられるものである。

８０は操作パネルであり、オペレータが操作してプレイ
バックモード、教示モード等を指示するもの、８１はメ
モリであり、教示データ等を格納するもの、８２はプロ
セッサ（以下ＣＰＵと称す）であり、マイクロプロセッ
サ等で構成され、プレイバンク時にメモリ８１の内容を
読出して各部へ指令を与えるもの、８３はＸ、Ｙ軸サー
ボ制御部であり、Ｘ軸モジュールｌａ、ｌｂのＸ軸モー
タ１０ａ、１０ｂ及びＹ軸モータ２４を位置制御するた
め、ＣＰＵ８２からの指令位置ＣＸ２、ＣＸ＋、ＣＹと
後述するドライバ位置検出回路からの現在位置ＰＸ２、
ＰＸｌ、ＰＹとの差を出力するもの、８４はパワーアン
プであり入力を増幅してＸ軸モータ１０ａ、１０ｂ、Ｙ
軸モータ２４に電流を供給するもの、８５は前述の合成
部であり、第８図にて後述する如く、Ｚ軸の指令値ＶＺ
と後述する力制御部の制御出力ＰＦＺとの差をとり、サ
ーボ回路へ与えるもの、８６は前述のサーボ回路であり
、第８図にて後述するもの、８７は力制御部であり、第
８図にて後述する様に力センサ３の検出出力ＦＺを受け
、これをデジタル値ＦＺに変換するとともに不感帯を設
定して制御出力ＰＦＺを出力するもの、８８はハンド位
置検出部であり、各軸のモータ１０ａ、１０ｂ、２４．
２６に設けられたロータリーエンコーダの出力から各軸
の現在位置ＰＸＩ、ＰＸ２、ＰＹ、ＰＺを求め、ハンド
５の現在位置を得るもの、８９はバスであり、ＣＰＵ８
２とメモリ８１、操作パネル８０、サーボ制御部８３、
合成部８５、力制御部８７及びハンド位置検出回路８８
とを接続し、データ、コマンドのやりとりを行なうもの
である。

第８図中、８５０はデジタル／アナログコンバータ（以
下Ｄ／Ａコンバータと称す）でありＣＰＵ８２からバス
８９を介して与えられる速度指令ＶＺをアナログの電圧
指令に変換するもの、８５１は差動アンプであり、Ｄ／
Ａコンバータ８５０の電圧指令と不感帯回路８７０から
の制御出力ＰＦＺとの差をとるもの、８６ｂは差分回路
であり、差動アンプ８５１の出力Ｖｃ’と実速度Ｖｒと
の差をとるもの。８７０は不感帯回路であり、ＣＰＵ８
２からバス８９を介し不感帯幅Ｗがセットされ、力セン
サ３の検出出力ＦＺが−Ｗ≦Ｍ≦Ｗにある時は、制御出
力ＰＦＺを零とし、ＭＯＷなら正の制御出力ＰＦＺを、
Ｍ＜−Ｗなら負の制御出力ＰＦＺを与え、合成部８５の
差動アンプ８５１に与えるもの、８７１はアナログ／デ
ジタルコンバータ（以下Ａ／Ｄコンバータと称す）であ
り、アナログの検出出力ＦＺをデジタル値に変換してバ
ス８９へ出力するものである。

次に、第９回吸着動作処理フロー図により係るロボット
の吸着取出し動作について説明する。

■先づ、ＣＰＵ８２は円板９のＸ、Ｙ座標位置を位置指
令ＣＸ１、ＣＹをバス８９を介しサーボ制御部８３ヘパ
ワーアンプ８４より駆動電流ＳＸ＋、ＳＹをＸ軸モータ
１０ａ、Ｙ軸モータ２４に供給する。これによって、Ｘ
軸モジュール１ａのＸ軸モータ１０ａ、Ｙ軸モータ２４
が駆動されて、真空吸着ハンド５はＸ軸モジュールＩａ
上のバンク）ｌｌａの治具６の円板９上にＸ−Ｙ位置決
めされる。

■次に、ＣＰＵ８２はハンド位置検出回路８８の各軸の
現在位置ＰＸ２、ｐｙ、ｐｚからの所定のハンド位置に
到達したか否かを調べ、そして所定の位置に停止した後
の所定の時間経過後（０，５秒程度）、力センサ３の振
動停止とみなし、カフィードパンクをオンとする。例え
ば、図示しない力制御部８７と合成部８５間のスイッチ
をオンとし、力制御部８７の出力ＰＦＺを合成部８５へ
入力可能とする。

■ＣＰＵ８２は、バス８９を介し指令値ＶＺを合成部８
５へ与える。前述の如く、円板９への接近中はＰＦＺ＝
０であるから、サーボ回路８６へは指令速度として出力
され、Ｚ軸モータ２６を速度制御する。従って、吸着ハ
ンド５は円板９に向って指令速度■１で下降する。

■一方、ＣＰＵ８２はバス８９を介し力制御部８７のＡ
／Ｄコンバータ８７１の力計測値ＦＺを監視し、ＦＺが
所定の値Ｍとなると、吸着ハンド５が円板９に接触した
と判定する。これ以降第５図で示した如く、カフィード
バンクが働き、合成部のアンプ８５１の出力Ｖｃ’が減
少し、２軸モータ２６、即ち吸着ハンド５の下降速度は
減少し、吸着ハンド５の円板９への押し付は力が発生し
、徐々に押し付は力は増加する。

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

■ＣＰＵ８２は、吸着ハンド５が円板９を吸着したと判
定すると、指令値ｖＺを零とし、次に前述のスイッチを
オフとしてカフィードバックをオフとする。

更にＣＰＵ８２は、吸着ハンド５を上昇すべく、つ１ 逆方向の指令値ＶＺをバス８９を介し合成部８５へ与え
、これによってＺ軸モータ２６は逆回転し、吸着ハンド
５は上昇し、円板９の取り出しが行なわれる。

更に前述と同様にＸ軸モータｌ　Ｏｂ、、　Ｙ軸モータ
２４を駆動して吸着ハンド５をＸ軸モジュール１ｂのパ
レットＩｌｂのベース７の所定の位置に位置決めし、ス
テップ■〜■と同様Ｚ方向に下降せしめ、ベース７への
接近、接触、押し付けを行い、吸着を解除して円板９の
ベース７への取付けを行う。

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

〔発明の効果〕

以上説明した様に、本発明によれば、物体への接近、接
触、押し付けがループの切換えなく連続的に円滑に行う
ことができるという効果を奏し、物体への急激な力を付
与することなく、円滑な動作ができ、特にロボット等の
取り出し、取り付は作業に好適である。

又、接触までは、指令値が速度指令なので接近の高速性
を損なうこともないという効果も奏し、高速駆動が可能
となる。しかも、係る動作を自律的に行うことができる
から、近接スイッチ等のアナログスイッチも設けなくて
も済むという効果も奏し、安価で、動作の安定した装置
を提供できる。

【図面の簡単な説明】

第１図は本発明の原理説明図、 第２図は本発明の一実施例構成図、 第３図は第２図構成の動作説明図、 第４図は本発明の他の実施例構成図、 第５図は第４図構成の動作説明図、 第６図は本発明をロボットに適用した構成図、第７図は
第６図構成のハンド及び力センサ構成図、 第８図は第６図構成の合成部及び力制御部ブロック図、 第９図は第６図構成における吸着動作処理フロー図、 第１０図は従来技術の構成図である。 図中、３−力センサ（力検出手段）、 ５−＝ハンド（移動体）、 ９−円板（物体）、 ２６−Ｚ軸モータ（駆動手段）、 ８５−・−合成部（制御手段）、 ８６−サーボ回路。

Claims (1)

1. 【特許請求の範囲】 移動体（５）を駆動する駆動手段（２６）と、該移動体
   （５）に加わる外力によって変位し且つ該外力を検出す
   る力検出手段（３）と、 該駆動手段（２６）を速度制御するためのサーボ手段（
   ８６）と、 指令値（Ｖｃ）と該力検出手段（３）の出力（Ｆｒ）と
   を合成した信号（Ｖｃ′）を該サーボ手段（８６）に供
   給する制御手段（８５）とを備え、該移動体（５）の物
   体に対する接近、接触、押し付けを連続的に行うことを
   特徴とする移動体制御装置。