JPH0721732B2

JPH0721732B2 - 移動体制御装置

Info

Publication number: JPH0721732B2
Application number: JP60104859A
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: JPS61262816A

Description

【発明の詳細な説明】〔目次〕概要産業上の利用分野従来の技術発明が解決しようとする問題点問題点を解決するための手段（第１図）作用実施例（ａ）一実施例の構成の説明（第２図，第３図）（ｂ）一実施例の動作の説明（第４図，第５図）（ｃ）他の実施例の説明（第６図，第７図）（ｄ）別の実施例の構成の説明（第８図，第９図，第10
図）（ｅ）別の実施例の動作の説明（第11図）（ｆ）更に別の実施例の説明（第12図）発明の効果〔概要〕移動体を物体との状態検出出力に応じて状態適応制御す
る移動体制御装置において，指令値と該状態検出出力と
の合成を速度指令として発生する制御手段と，該速度指
令に追従してサーボ制御するサーボ回路とを設け，サー
ボ回路を関数発生部と閉ループ制御部とで構成すること
によつて，円滑で自律的な状態適応制御を行なうととも
に状態検出出力の過大な入力に対しても安定に動作でき
るようにしたものである。

〔産業上の利用分野〕

本発明は，ロボツト等における作業部（ハンド）等の移
動体を物体に対する状態に応じて状態適応制御する移動
体制御装置に関し，特に状態適応制御を円滑に行なうと
ともに状態量に対して安定な動作が可能な移動体制御装
置に関する。

近年，ロボツトの高機能化の開発が目覚しく，作業内容
の高度化，高精度化を目標とする試みが種々なされてい
る。例えば，刊行物「日経メカニカル1985.4.8」（1985
年４月８日日経マグロウヒル社発行）の第73頁乃至第81
頁や刊行物「ロボツト工学入門」（昭和58年９月10日オ
ーム社発行」等においては，ロボツトに対し対象物体に
対する視覚，力覚等のセンサを付与し，これらのセンサ
からの状態に応じて適応制御を行なう技術が開示されて
おり，これによつて物体との相対位置誤差の補正等を行
なうようにしている。

〔従来の技術〕

係る移動物体，特にロボツトの制御においては，一般に
外的な状態に拘束されない位置制御と外的状態による適
応制御を行なうようにしている。例えば，ハンドに力セ
ンサによる力覚を設けたものにおいては，ハンドが対象
物体に当接するまで位置制御を行ない，当接すると力セ
ンサによる対象物体からの反力をフイードバツクし，反
力を零とするように適応制御して，ハンドに把持した物
品を対象物体にはめ合わせる等の作業を行ない，相対位
置ずれを補正又は吸収するようにしている。

又，視覚を備えるものにおいては，視覚（例えばカメ
ラ，距離センサ等）による出力によつて位置指令を変更
しながら，対象物体に近づくことにより，指示した位置
を補正する方法が用いられている。

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

しかしながら，従来の前者の構成においては，指令（位
置）制御から力制御へ物体との接触時に切り替えを行な
うことから，切り替えによつて動作が不連続となり，系
が不安定となり接触直後に過度現象として急激な制御量
が入力されるおそれがあるという問題があり，これを避
けるには，物体との接触後十分な時間を置いた後に力制
御へ切り替える必要があり，高速な動作ができないとい
う問題があつた。

一方，後者の構成では，状態フイードバツクに応じて常
に指令を変更しているので，高速動作が困難であり，又
過大な状態フイードバツクに対しては，系を安定動作さ
せるため何等かのリミツトを設けて指令を変更する必要
があり，この処理により一層高速動作が困難であるとい
う問題も生じていた。

本発明は，円滑に且つ自律的に状態適応制御を行なうと
ともに状態量にかかわらず安定な動作が可能な高精度，
高速動作を達成しうる移動体制御装置を提供することを
目的とする。

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

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

図中,3は状態センサ（検出手段）であり，例えば移動体
に加わる外力を検出する力センサで構成されるもの,5は
移動体であり，例えば物品を吸着保持する吸着ハンドで
構成されされるもの,9は対象物体であり，例えば，磁気
デイスク等のハンド５によつて吸着されるもの,MTは駆
動部であり，モータ等のアクチュエータで構成され，状
態センサ３の設けられたハンド５を移動するもの,82は
制御部であり，移動指令Vcと状態センサ３からの状態量
Ｆとを合成して駆動部MTを制御するものであり，合成部
85を有するもの,SVCはサーボ回路であり，制御部82から
の速度指令Vc′に追従して駆動部MTをサーボ制御するも
のであり，速度指令Vc′に対し所定の加速度をもつて追
従する速度関数を発生し，速度関数の積分値である比較
位置関数を発生する関数発生部83と，比較位置関数と駆
動部MTの検出位置を比較して制御入力を駆動部へ与える
閉ループ制御部84とを有するものである。即ち，本発明
では，移動指令Vcと状態指令Ｆとの合成を速度指令とす
る制御部82と，関数発生部83及び閉ループ制御部84とか
らなるサーボ回路SVCとが結合されて構成されている。

〔作用〕

本発明では，移動指令と状態指令Ｆとの合成をサーボ回
路SVCの入力としている。従つて状態指令が発生されな
い状態（即ち，零）においては移動指令がサーボ回路SV
Cへ入力され，指令制御が実行され，一方，状態指令が
発生されると，移動指令と状態指令との合成がサーボ回
路SVCへ入力され，いわゆる状態適応制御が実行され
る。

従つて指令制御から状態適応制御への移行が切替えなし
に又移動指令を変更せず，円滑に自律的に実行されるこ
とになる。

一方，このような状態適応制御においては，状態量（状
態指令）は系からみると外乱であり，必ずしもリニアな
特性をもつものでなく，物体との状態においては，状態
量が急激に大となる場合がある。

従つて合成指令が急激に変動して追従制御が困難とな
り，円滑な速度減少特性を得ることができない。このた
め，関数発生部83によつて合成速度指令を所定の加速度
の速度関数に変換することによつて加速度成分が一定値
以上にならないようにして閉ループ制御部84の飽和を防
止し，円滑な速度変化を得るようにしている。更に閉ル
ープ制御部84では，関数発生部83から積分した基準位置
関数と検出位置との差で制御入力を作成しているので零
に近い低速の速度指令に対しても正確な移動指令が作り
出せるから，移動指令Vcと状態指令Ｆとの差が零に近づ
いても正確に追従駆動することができる。

即ち，本は対は系から見て不定な外乱である状態量を単
にフイードバツクして自律的な状態適応制御を行なうよ
うにしているが，この状態量は不定で且つ外乱であるか
ら，この外乱を入力しても安定に系が動作しないと，状
態適応制御が円滑に実行できないため，サーボ回路に係
る外乱の吸収追従動作を行わせ，安定な動作を実現する
ものである。

〔実施例〕

（ａ）一実施例の構成の説明。

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

図中，第１図で示したものと同一のものは同一の記号で
示してあり,82は制御部としてのプロセツサ（以下CPUと
称す）であり，マイクロプロセツサで構成され，力計測
値Frに所定の利得αを利得手段82aで乗算し，乗算出力
α・Frを入力指令Vcから合成手段85で差し引き指令出力
Vc′を発するものであり,3は力センサであり，平行板バ
ネと歪ゲージ30により構成され，外力によつて変位し且
つ変位を歪ゲージ30で検出するもの,26はＺ軸モータで
あり，送りネジ26bを回転させ力センサ３に支持された
ハンド５をＺ軸方向に移動するものであり，位置検出器
（エンコーダ）26aが設けられたもの,9は磁気デイスク
板（以下円板と称す）であり，スペーサ90を介在して複
数枚積層され，ハンド５によつて吸着されるものであ
る。

この例では，状態としてハンド５の物体（円板９）との
接触及び反力を検出する力センサ３が状態検出手段とし
て設けられ，ハンド５は力センサ３を介しＺ軸モータ26
によつてＺ軸方向の上下動可能である。

第３図は第２図構成のサーボ回路の回路図である。

図中，第１図及び第２図で示したものと同一のものは同
一の記号で示してあり,830は速度関数発生部であり，速
度指令Vc′（（ｔ））に対し所定の加速度の速度関数
を発生するものであり，所定の加速度に対応する周期の
パルスを発振するオシレータ（OSCという）830aと,OSC8
30aのパルスをアツプ又はダウンカウントする速度カウ
ンタ830bと，速度指令（ｔ）と速度カウンタ830bの内
容（追従速度関数）Ｒを比較する比較器830cと，比較
器830cの出力によつてOSC830aのパルスを速度カウンタ8
30bのアツプ側又はダウン側に与えるスイツチ830dとを
有するものであり，（ｔ）＞ＲならOSC830aのパル
スをアツプ側に切換え，速度カウンタ830bを加速計数
し，（ｔ）＝ＲならOSC830aのパルスを速度カウン
タ830bへ入力せず，その内容を保たせ，（ｔ）＜Ｒ
ならOSC830aのパルスをダウン側に切換え，速度カウン
タ830bを減速計数させるものである。831は速度／位置
切換スイツチであり，後述する比較器の制御（点線）に
より速度カウンタ830bと後述する目標位置関数発生部と
を切換えるもの,832は比較位置関数発生部であり，切換
スイツチ831を介して与えられる２値の速度関数を積分
して比較位置関数θＲ（ｔ）を発生するものであり,2値
の速度関数とOSC830aのパルス周波数の積を演算して速
度関数に応じた周波数のパルスを出力するバイナリーレ
イトマルチプライヤ（以下BRMという）832aと,BRM832a
の出力パルスを計数する比較位置カウンタ832bとを有す
るものである。833は比較加速度関数発生部であり，比
較加速度関数を速度カウンタ830bの加速，停止又は定
速，減速に応じて出力するものであり，スイツチ830dの
状態（加速，停止又は定速，減速）に応じて正値，零，
負値の比較加速度関数Ｒ（ｔ）を出力する加速度関数
生成部833aと，比較加速度関数Ｒ（ｔ）に対しJ/Ae・
A_M（但し,Jはイナーシヤ,Aeはパワーアンプ利得，A_Mは
モータの力定数である）を乗じて電流入力UR（ｔ）を発
生する比例乗算部833bを有するものである。

834は比較器であり速度カウンタ830bの値Ｒと後述す
る目標位置関数発生部835からの減速速度ｄとを比較
して一致すると，スイツチ831を目標位置関数発生部835
側に切換えるためのもの,835は目標位置関数発生部であ
り，目標である位置指令θ（ｎ）とカウンタ832bの比較
位置関数θＲ（ｔ）との差に対応する減速速度信号ｄ
を発生するものであり，θ（ｎ）とθＲ（ｔ）の差をと
る差分器83aと，差分器835aの差に応じた目標位置に位
置決めするための減速信号ｄを発生するリードオンリ
ーメモリ（以下ROMと称す）835bとを有するものであ
る。

840は位置カウンタであり，モータ26の位置検出器（エ
ンコーダ）26bのパルスを計数し現在位置θｐを表わす
もの,841は差分器であり，比較位置関数θＲ（ｔ）と位
置カウンタ840の現在位置θｐとの差をとるもの,842は
デジタル／アナログコンバータ（以下D/Aコンバータと
称す）であり，差分器の位置（角度）誤差をアナログに
変換するもの,843はスムーザであり，エンコーダ26b角
度検知の量子化誤差によりリミツトサイクルの影響を防
ぐため，エンコーダ26bのパルス間を補間して位置誤差
θｅ（ｔ）を連続的なものとするためのものであり,844
は加算器であり，スムーザ843の出力をアナログの位置
誤差に加算し連続的な娯差θｅ（ｔ）を得るものであ
る。845は制御入力発生部であり，位置誤差θｅ（ｔ）
から制御入力Ue（ｔ）を作成するものであり，位置誤差
θｅ（ｔ）を積分する積分器845aと，位置誤差θｅ
（ｔ）及び制御入力Ue（ｔ）を入力とし，制御対象（即
ち，パワーアンプ，モータ26,エンコーダ26b）と同一の
系を構成して推定角速度偏差₂（ｔ）と推定角度偏差
₃（ｔ）を発生するオブザーバ（状態観測器）845b
と，各々積分量₁（ｔ）推定角度偏差₂（ｔ），推定
角速度偏差₃（ｔ）にフイードバツク係数f₁，f₂，f₃
を乗じる乗算器845c〜845eと，乗算器845cの出力f₁・x₁
（ｔ）と乗算器845dの出力f₂・₂（ｔ）とを負の加算
を行う加算器845fと，加算器845fの出力（−f₁・x
₁（ｔ）−f₂・₂（ｔ））から乗算器845eの出力f₃・
₃（ｔ）を減算する減算器845gとを有するものであり，
従つて制御入力Ue（ｔ）は Ue（ｔ）＝−f₁・x₁（ｔ）−f₂・₂（ｔ）−f₃・
₃（ｔ）となる。

846は加算器であり，電流入力UR（ｔ）と制御入力Ue
（ｔ）を加算し，入力Ｕ（ｔ）を出力するもの,847はパ
ワーアンプであり，電流駆動形パワーアンプで構成さ
れ，入力Ｕ（ｔ）に比例した電流をモータ26に供給でき
るよう内部で電流帰還されているものである。

（ｂ）一実施例構成の動作の説明第３図のサーボ回路SVC自体は本発明者等によつて提案
され，周知であるが，簡単に第４図を用いて説明する
と，関数発生部83は，速度（角速度）指令（ｔ）と位
置指令（ｔ）を受けると，速度関数発生部830は第４
図（Ａ）の如くステツプ状の速度指令（ｔ）に対し所
定の加速度（OSC830aのパルス周波数で決定される）で
追従する速度関数Ｒ（ｔ）を発生し，この速度関数
Ｒ（ｔ）は比較位置関数発生部832で時間積分され，比
較位置関数θＲ（ｔ）が出力される。一方，比較加速度
関数部833からは，速度関数Ｒ（ｔ）の加速，減速に
応じ比較加速度関数Ｒ（ｔ）を発生し，更にこれによ
つて電流入力UR（ｔ）を出力する。一方，目標位置関数
発生部835は目標位置θ（ｎ）と比較位置関数θＲ
（ｔ）の差に対応する減速信号ｄ（第４図（Ａ））を
発生し，比較器834が速度関数Ｒ（ｔ）と減速信号
ｄとの値が一致すると（時刻t₁），スイツチ831を目標
位置関数発生部835側に切換え減速信号ｄを比較位置
関数発生部832に与えてこれを時間積分して比較位置関
数Ｒ（ｔ）を出力せしめる。従つて関数発生部83は速
度指令（ｔ）に対し所定の加速度で追従する速度関数
Ｒ（ｔ）を発生し，この速度関数に応じて比較加速度
関数Ｒ（ｔ）を指令入力UR（ｔ）として発生し，且つ
速度関数Ｒ（ｔ）から比較位置関数θＲ（ｔ）を作成
している。

一方，閉ループ制御部84では，比較位置関数θＲ（ｔ）
と現在位置θｐとの位置誤差θｅ（ｔ）から制御入力Ｕ
（ｔ）作成し，パワーアンプ847へ帰還している。従つ
て比較位置関数θＲ（ｔ）が基準となつて閉ループ制御
され，比較位置関数θＲ（ｔ），即ち速度関数Ｒ
（ｔ）の積分値に正確に追従した駆動制御が可能とな
る。この制御入力発生部845では位置誤差θｅ（ｔ）を
積分しており，これによつてモータ軸のまさつ力や外力
に打ち勝つた駆動ができる。

このような特性を有する安定なサーボ回路によつて，次
のような状態適応制御が行なわれる。

ハンド５が円板９に接近中では，状態量としての力セン
サ３の力計測値Frは零であるから,Vc′＝Vcであり,Z軸
モータ26はサーボ回路86によつてパワーアンプ86aを介
し速度制御され，ポールネジ26bを回転させて，力セン
サ３を介しハンド５を下降せしめる。

一方，ハンド５が円板９に位置P₁で接触すると，力セン
サ３は板バネで構成されているからたわみ（変位し），
このたわみを検出する歪ゲージ30より力計測値Frが発生
する。この力計測値Frは必ずしもリニアな特性を有する
ものではなく，第５図（Ａ）に示す如く，接触具合によ
つて急激に上昇したり，振動等によつてふらつく。

CPU82は，合成部85より（Vc−α・Fr）なる速度指令V
c′（（ｔ））を発生し,Vc′は力計測値Frに従つて減
少する。この時，力計測値Frが急激に変化したとする
と，速度指令Vc′も急激に変化し，その加速度成分であ
る変化率は無限大になるので，この速度指令にそのまま
サーボモータ26を追従させようとすると無限大の電流が
要求されることになる。しかしながら，実際には有限の
加速度しか得られないので，他のサーボ系では追従遅れ
が出て，位置偏差が大きくなり,D/Aコンバータの飽和が
おこり，暴走の危険があるため，緊急停止となる。（他
のサーボの方式でも，偏差カウンタ方式では偏差カウン
タの飽和,PLL方式では脱調，パルスモータでの脱調など
か生じる。）第３図のサーボ回路では速度指令Vc′の指示に対して，
一定の加速度を持つてこれに追従する速度関数を発生す
ることにより，力計測値の急激な変化に対して，サーボ
系が常に保護されるようになつている。

又。力計測値Frのふらつきに対してもこれを吸収し，従
つて第５図（Ｂ）の如くモータ26は円滑に減速し始め
る。

又，この時，モータ26はハンド５の円板９への接触によ
り大きな外力を受けるが，閉ループ制御部84の動作によ
つてこれに打ち勝ち関数発生部83の速度関数に従つて動
作する。

この時，減速の傾きは利得αで制御され，平行板バネが
モータ26の回転で更にたわみ，力計測値Frは上昇する。
最終的には位置P0で実速度Vrが零となり，この時,Vc＝
α・Frであり,F0の押し付け力が発生している。

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

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

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

この位置P₀においても，サーボ回路SVCの特性が発揮さ
れる。即ち，正確な力制御のためには,Vc＝α・Frが必
要であり，零に近い低速の速度指令に対しても正確にモ
ータを駆動しなければならない。このサーボ回路SVCで
は関数発生器83で比較位置関数θＲ（速度関数Ｒの積
分値）を作成しているので，微少な速度指令Vc′でも正
確な制御入力Ｕ（ｔ）を閉ループ制御部84で作り出せ，
これは比較位置関数θＲに追従するので，まさつ力や外
力に打ち勝つた正確かつ精密な制御ができる。

従つて，接近中は指令速度Vcによつて速度制御され，ハ
ンド５が円板９に接触すると，外力適応制御に自律的に
移行し，最終的な平衝状態では，時刻t₀で位置P₀でＺ軸
モータ26が停止し，実速度値が零となる。この時入力指
令Vcと力計測値Frが等しく，力計測値Frは力センサ３の
変形によつて物体９への押し付け力F0となるから，入力
指令Vcは力指令として働く。従つて，入力指令Vcの大き
さが押し付け力を制御することになる。

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

このようにして接近，接触，押し付けの３過程が連続的
に円滑に行われ且つ力センサ３の力指令値に急激な変化
が生じても安定に且つ円滑に進行することができる。即
ち，サーボ回路SVCの閉ループ制御器84の帰還ループを
設けていることにより，まさつ力などのさまざなな外乱
を抑圧して，関数発生部83の位置の指令（比較位置関
数）に正確に追従する理想的なサーボとなつており，き
わめて精密な速度制御が可能なこと，サーボ系が飽和し
ないように関数発生部83によつて加速度成分が一定値以
上にならないよう監視された形で速度をなめらかに変化
させていくことができる。

このサーボ回路は位置指令による位置制御もできるよう
になつており，位置制御が不要であれば，位置関数発生
部835,比較器834,スイツチ831は不要であり，カウンタ8
30bの出力を比較位置関数発生部832に直接入力すればよ
い。

このような動作は，力センサ３の如く接触型センサに限
らず，物体との状態を検出する非接触型センサ（例えば
超音波距離センサ等）を用いた場合にも同様であり，こ
れらによる状態適応制御にも適用できる。

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

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

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

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

この実施例では，不感帯部82bによつて希望の速度と希
望の押し付け力を得るようにしている。

即ち，不感帯部82bは，力計測値Frに対し非線形要素を
通した信号のフイードバツクを実行するものであり,CPU
82の演算によつて行われる。

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

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

次に，第６図実施例構成の動作について第７図を用いて
説明する。

第７図に示す如く位置P₁で接触しても不感帯幅Ｗに対応
する押し付け力になるまで指令Vcで与えられた速度で移
動を続け，その後は，即ちFr＞Ｗとなることによつて第
３図と同様に減速して停止する。

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

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

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

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

別の実施例として,CPU82内で利得制御及び不感帯制御の
両方を行つてもよく，利得手段82a又は不感帯部82bをほ
CPU82のプログラムの実行によらず別のハードウエアで
構成してもよい。

（ｄ）別の実施例の構成の説明。

第８図は本発明を直交ロボツトに適用した場合の別の実
施例の構成図，第９図は第８図構成の力センサ構成図，
第10図は第８図構成の合成部，サーボ制御部及び力制御
部のブロツク図である。

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

80は操作パネルであり，オペレータが操作してプレイバ
ツクモード，教示モード等を指示するもの,81はメモリ
であり，教示データ等を格納するもの,82はプロセツサ
（以下CPUと称す）であり，マイクロプロセツサ等で構
成され，プレイバツク時にメモリ81の内容を読出して各
部へ指令を与えるもの,SVCはサーボ制御部であり,X軸モ
ジユール1a,1bのＸ軸モータ10a,10b及びＹ軸モータ24,Z
軸モータ26及びγ軸モータ４を位置，速度制御するた
め,CPU82からの指令位置CX₂，CX₁,CY,CZ,Cγが入力さ
れ，後述するハンド位置検出回路からの現在位置PX₂，P
X₁,PY,PZ,Pγがフイードバツクされ，更に合成部から指
示速度V_cx〜V_c _γが入力され，これらによつてサーボ制
御するものであり，前述の関数発生部83と閉ループ制御
部84で構成されるもの,85は前述の合成部であり，第10
図にて後述する如く，各軸の指令速度V_x1〜Ｖ_γと後述
する力制御部の力制御出力PF_x〜PR_γとの差をとり，サ
ーボ制御部SVCの関数発生部83へ与えるもの,87は力制御
部であり，第10図にて後述する様に力センサ３の検出出
力F_x〜Ｆ_γを受け，これをデジタル値F_x〜Ｆ_γに変換す
るとともに不感帯を設定して制御出力PF_x〜PF_γを出力
するもの,88はハンド位置検出部（閉ループ制御部84の
カウンタ840）であり，各軸のモータ10a,10b,24,26,4に
設けられたロータリーエンコーダ26b等の出力から各軸
の現在位置PX₁，PX₂,PY,PZを求め，ハンド５の現在位置
を得るもの,89はバスであり,CPU82とメモリ81,操作パネ
ル80,サーボ制御部83,合成部85,力制御部87及びハンド
位置検出回路88とを接続し，データ，コマンドのやりと
りを行なうものである。

第９図は第８図構成における４自由度力センサ３の構成
図である。

力センサ３は,X,Y,Z軸の外力を検出するX,Y,Z力検出モ
ジユール740と，γ軸の外力を検出するγ力検出モジユ
ール750とで構成される。

力検出モジユール740は，第９図から明らかな如く，各
平行板バネ体が変位方向が互いに直交するように設けら
れているので，平行板バネ体a1,a1′でＸ軸方向のたわ
み，平行板バネ体b1,b1′でＹ軸方向のたわみ，平行板
バネ体c1,c1′でＺ方向のたわみを夫々分担する３自由
度を有する。

743,744は夫々力検出モジユール740を支持する支持体で
あつて，支持体743はねじ745により角棒742と連結さ
れ，支持体744はねじ746により角棒741と連結されてい
る。尚，ねじ745,746は片方のみ示し，さらに，各ねじ7
45が螺合するねじ穴743aと他方の穴は中心穴740aの中心
位置から等しい距離の位置に設定され，同様にねじ746
が螺合するねじ穴744aと744bは中心穴250aの中心位置か
ら等しい距離（L9＝L10）の位置に設定されている。

747は支持体743にねじ748により連結される出力棒であ
つて，力検出モジユール740に設けられた穴740aを貫通
するように構成されている。

この場合，支持体744が真空吸着ハンド５に固定され
る。

尚，出力棒747は力検出モジユール740に設けられた穴74
0aを貫通するよう構成されているが，支持体744を貫通
するように構成してもよく，この場合は，支持体744の
基台への取付けを反対側（角棒741,742側）で連結する
必要がある。

749a,749b,749c,749d,749e,749fは歪ゲージであつて，
夫々各平行板バネa1′,b1,c1′の変位を検出する。ここ
で，この歪ゲージは軸方向の力をトルクの影響を受けず
に検出するため，中心穴740aを中心として中心点対象と
なるように貼付し，夫々ブリツジ回路を構成せしめる。

従つて，図示されていないが，平行板バネ体a1,c1,b1′
にも歪ゲージが中心穴740aの中心点対象位置となるよう
に各々２枚づつ貼付されている。

以上説明した構成とすることにより，たとえば，出力棒
747はＸ軸方向の力が加わつた場合，歪ゲージ749c,749d
が平行板バネc1′の変位を検出し,X軸方向のみの力を検
出でき，同様にＹ軸方向の力が加わつた場合歪ゲージ74
9e,749fが平行板バネb1の変位を検出し,Z軸方向の力が
加わつた場合歪ゲージ749a,749bが平行板バネa1′の変
位を検出し，各軸の力成分を検出する。

さらに複数方向の合力が加わつた場合でも，角棒741,74
2に加わる力の位置は中心穴740aの中心位置から等しい
距離の位置に支持体743,744により加わるため，各平行
板バネ体が夫々の分力Fx,Fyを独立して検出することが
できる。

750はγ力検出モジユールであつて，力検出モジユール7
40の出力棒747にねじ751を介して取付けられる中心部材
752を備えると共に，板バネ750a,750b,750c,750dを介し
て接続される外輪753を含む。754a,754b,754c,754dは歪
ゲージであつて，板バネ750a,750cに貼付（中心部材752
の中心点対象位置で，同一面側）され，同様にブリツジ
回路を構成する。

尚，γ力検出モジユール750の出力棒747への取付けは，
ねじ751のみで出力棒747の中心位置としているが，この
構成では外輪753にトルクを与えた際に，ねじ751のゆる
み等が生じるため，実際には，中心部材752から突出す
るピンを出力軸757に係合させてまわり止めを施すと共
に，中心位置からずれたところでねじ751により固定す
る必要がある。

また，このことは，出力軸757と支持体754との結合の場
合も同様である。

この構成とすることにより中心部材752を固定し，外輪7
53に中心軸（γ軸）まわりのトルクを加えると板バネ75
0a,750b,750c,750dがたわむ。このたわみを歪ゲージ754
a,754b,754c,754dで検出し，ブリツジ回路を介して出力
を取り出すことにより,Z軸（γ）に関するトルクＴ_γの
みを検出することができる。

第10図は力制御部87,合成部85,サーボ制御部SVCの詳細
回路図である。

図中，第８図で示したものと同一のものは同一の記号で
示してあり,800〜803は各軸の合成回路であり，力制御
部87から力制御指令PF_x〜PF_γとCPU82からの速度指令V_x
〜V_zとの差を検出するもの,804〜807は不感帯部であり,
CPU82からの不感帯幅W_x〜Ｗ_γが設定され，力計測値F_x
〜Ｆ_γに対し不感帯を与えるもの,808はスイツチであ
り，力計測値F_x〜Ｆ_γの入力の許可／不許可をするもの
であり，力フイードバツクオン／オフをCPU82の制御に
より行うもの,809はアナログ／デジタルコンバータ（A/
Dコンバータと称す）であり，力センサ３からのアナロ
グの力計測値FX,FY,FZ,Fγをデジタル値に変換して，ス
イツチ808に出力するもの,83a〜83dは各々各軸の関数発
生部であり，指令位置CX,CY,CZ,Cγと合成樹脂速度Ｖ′
ｘ〜Ｖ′γが入力されるもの,84a〜84dは各々各軸の閉
ループ制御部であり各軸のモータを駆動するもの,88は
前述の位置検出器であり，各軸モータに設けられたエン
コーダの出力を計数し，各軸の現在位置を検出するもの
である。

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

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

尚,X軸はX1軸とX2軸の２つがあるが,1つのＸ軸関数発生
部83a,閉ループ制御部84aで示してあり実際には２つあ
る。

（ｅ）別の実施例の動作の説明。

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

先づ,CPU82は円板９のX,Y座標位置及び所定のＺ座標
位置として位置指令CX₁,CY,CZをバス89を介し，関数発
生部83へ与え且つ合成部85にX,Y,Z速度指令VX₁,VY,YZを
与える。

これによつて，関数発生部83より閉ループ制御部84を介
し駆動電流SX₁,SY,SZがＸ軸モータ10a,Y軸モータ24,Z軸
モータ26に供給される。これによつて,X軸モジユール1a
のＸ軸モータ10a,Y軸モータ24,Z軸モータ26が駆動され
て，真空吸着ハンド５はＸ軸モジユール1a上のパレツト
11aの治具６の円板９上の所定のＺ位置（第２図の点
Ｐ）にＸ−Ｙ位置決めされる。

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

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

一方,CPU82はバス89を介し力制御部87のA/Dコンバー
タ809の力計測値FZを監視し,FZが所定の値Ｍとなると，
吸着ハンド５が円板９に接触したと判定する。これ以降
第６図で示した如く，フイードバツクによる適応制御が
働き，合成部802の出力VZ′が減少し,Z軸モード26,即ち
吸着ハンド５の下降速度は減少し，吸着ハンド５の円板
９への押し付け力が発生し，徐々に押し付け力は増加す
る。

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

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

更にCPU82は，吸着ハンド５を上昇すべく，逆方向の速
度指令値VZをバス89を介し合成部85へ与え，これによつ
てＺ軸モータ26は逆回転し，吸着ハンド５は上昇し，円
板９の取り出しが行なわれる。

更に前述と同様にＸ軸モータ10b,Y軸モータ24を駆動し
て吸着ハンド５をＸ軸モジユール1bのパレツト11bのベ
ース７の所定の位置に位置決めし，ステツプ〜と同
様Ｚ軸方向に下降せしめ，ベース７への接近，接触，押
し付けを行い，吸着を解除して円板９のベース７への取
付けを行う。

従つて，指令制御と力フイードバツク制御とが選択的に
実行され，且つ円滑な外力適応制御ができる。

（ｆ）更に別の実施例の説明。

第12図はCPU82の処理説明図である。

この例では，合成部85及び力制御部87の機能をCPU82の
プログラムの実行によつて行なうものである。

CPU82はメインルーチンにおいて，教示データのコマン
ドを解析し，これを実行して各軸の位置指令，速度指
令，力指令を作成し，バス89を介し関数発生部83へ出力
し，又位置検出器88からの現在位置Ｐ_γ〜P_x2によつて
各軸の位置及び力センサ３からの力計測値Ｆ_γ〜F_xを監
視する。そして，通常（指令）モードでは，スイツチの
点線の如く指令位置と指令速度をそのまま関数発生部83
へ与えて，閉ループ制御部84を介し各軸を指令位置に位
置決めする。一方，力フイードバツクモードにおいて
は，所定周期で力フイードバツク制御の割込み処理ルー
チンを実行する。即ち，力センサの力計測値Ｆ_γ〜F_xを
オフセツト補正し，さらに不感帯処理して，帰置利得を
掛け制御出力を得，これを指令速度又は指令力から差し
引いたものを指令速度Ｖ′_γ−Ｖ′_x2としてスイツチ実
線の如くバス89を介し関数発生部83へ与える。

この力フイードバツク制御によつて前述の如く次の様な
制御ができる。

この不感帯処理においては，不感帯幅Ｗ_γ〜W_xを自由に
設定でき，セツトされた不感帯幅がＷである時は，力計
測値Ｆが正なら（Ｆ−Ｗ）を出力し，負なら（Ｆ＋Ｗ）
を出力するものである。

このカフイードバツク制御によつて前述の如く次の様な
制御ができる。

ある軸（例えばＺ軸）に指令速度V_zを与えると，物品へ
の接近中は，指令速度V_zで移動する。一方，物品に点P₁
で接触すると，力センサがたわんで変形し，変形量に応
じた力計測値F_zが発生するが不感帯幅Ｗの範囲では制御
出力P_zは零のため，速度はV_zのままとなり，F_z＞Ｗとな
ると制御出力P_zが発生し，指令速度V_zを減少していく，
従つてＺ軸の移動速度も減少し，P_z＝V_zとなると,Z軸は
停止する。この時Pzの押し付け力が物品へ付与されてお
り，従つて指令速度V_zは指令力となつている。これによ
つて物品への接近，接触，押し付けが連続的に実行され
る。

これを利用して前述の物品の吸着，挿入，ねじの吸着，
ねじ締め等を円滑に行なうことができる。

又，位置指令，速度指令が与えられていない軸（例え
ば,X,Y軸）においてその軸方向に外力が与えられると，
力センサのたわみによる力計測値Ｆによつて制御出力が
発生し，これが指令速度となつて力計測値Ｆが零となる
方向にその軸が駆動される。このことを利用して，ハン
ドと物品のはめ合せ時にハンドと物品が接触してはめ合
せが困難となつても，力センサの出力が零となる方向に
自動的に駆動され，円滑なはめ合せが可能となり，物品
の吸着，挿入がより一層円滑となる。

前述の実施例においては，力センサによる力フイードバ
ツクを例に説明したが，他の接触センサによる状態フイ
ードバツクであつてもよく，更に非接触センサによる状
態フイードバツクでもよい。また，作業内容も物品の取
出しに限らず，物体への倣い動作等であつてもよい。

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

〔発明の効果〕

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

速度指令と力センサの状態出力とを合成しているの
で，円滑且つ自律的に移動体への接近，接触，押し付け
の工程が実現できる。

関数発生部により合成速度指令を所定の加速度の速度
関数に変換するため，不安定な力センサの出力が急激に
変動しても，円滑な速度変化が得られる。

閉ループ制御部が関数発生部の出力で制御入力を作成
しているので，零に近い低速の速度指令に対しても正確
な移動指令を作成でき，正確に力追従制御できる。

力センサの出力に不感帯を付与しているので，移動体
の接近中のドリフトを防止でき，且つ適切な押し付け力
を付与できる。

【図面の簡単な説明】

第１図は本発明の原理説明図，第２図は本発明の一実施例構成図，第３図は第２図構成のサーボ回路の回路図，第４図は第３図サーボ回路の特性図，第５図は第２図構成の動作説明図，第６図は本発明の他の実施例構成図，第７図は第６図構成の動作説明図，第８図は本発明の別の実施例構成図，第９図は第８図構成の力センサ構成図，第10図は第８図構成の合成部及び力制御部ブロツク図，第11図は第８図構成における吸着動作処理フロー図，第12図は本発明の更に別の実施例説明図である。図中,3……力センサ（状態検出手段）,5……ハンド（移
動体）,9……円板（物体）,26……Ｚ軸モータ（駆動手
段）,82……制御部,85……合成部,SVC……サーボ回路,8
3……関数発生部、84……閉ループ制御部。

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

Claims

【特許請求の範囲】

【請求項１】移動体（５）を駆動する駆動手段（MT）
と、該移動体（５）の該物体からの外力を検出する外力検出
手段（３）と、該駆動手段（MT）を速度制御するためのサーボ回路（SV
C）と、速度指令値から該外力検出手段（３）の出力に不感帯を
付与した出力を減じた合成速度信号（Vc′）を該サーボ
回路（SVC）に供給する制御手段（82）とを備え、該サーボ回路は、該合成した速度信号に対し所定の加速
度をもって追従する速度関数を発生する関数発生部（8
3）と、該速度関数を積分した比較位置関数と該駆動手段の検出
位置とを比較して該駆動手段に制御入力を与える閉ルー
プ制御部（84）とを有し、該物体への接近、接触、押し付けを連続的に行うことを
特徴とする移動体制御装置。