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

Abstract

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

Description

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

〔産業上の利用分野〕

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

近年、ロボットの高機能化の開発が目覚しく。

作業内容の高度化、高精度化を目標とする試みが種々な
されている。例えば、刊行物［日経メカニカル１９８５
．４．８Ｊ　（１９８５年４月８日　日経マグロウヒル
社発行）の第７３頁乃至第８１頁や刊行物「ロボット工
学入門」（昭和５８年９月ｌＯ日オーム社発行」等にお
いては、ロボットに対し対象物体に対する視覚、力覚等
のセンサを付与し。

これらのセンサからの状態に応じて適応制御を行なう技
術が開示されており、これによって物体との相対位置誤
差の補正等を行なうようにしている。

〔従来の技術〕

係る移動体、特にロボットの制御においては。

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

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

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

しかしながら、従来の前者の構成においては。

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

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

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

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

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

図中、３は状態センサ（検出手段）であシ１例えば移動
体に加わる外力を検出する力センサで構成されるもの、
５は移動体であり９例えば物品を吸着保持する吸着ノ・
ンドで構成されるもの、９は対象物体であり１例えば、
磁気ディスク等のハンド５によって吸着されるもの、Ｍ
Ｔは駆動部であり、モータ等のアクチュエータで構成さ
れ、状態センサ３の設けられたハンド５を移動するもの
。

８２は制御部であり、移動指令Ｖｃと状態センサ３から
の状態蓋Ｆとを合成して駆動部ＭＴを制御するものであ
り１合成部８５を有するもの、ＳＭＣはサーボ回路であ
り、制御部８２からの速度指令■ｃ′に追従して駆動部
ＭＴをサーボ制御するものであり、速度指令Ｖｃ’に対
し所定の加速度をもって追従する速度関数を発生し、速
度関数の積分値である比較位置関数を発生する関数発生
部８３と、比較位置関数と駆動部ＭＴの検出位置を比較
して制御入力を駆動部へ与える閉ループ制御部８４とを
有するものである。即ち１本発明では、移動指令Ｖｃと
状態指令Ｆとの合成を速度指令とする制御部８２と、関
数発生部８３及び閉ループ制御部８４とからなるサーボ
回路ＳＶＣとが結合されて構成されている。

〔作　用〕

本発明では、移動指令と状態指令Ｆとの合成をサーボ回
路８ＶＣの入力としている。従って状態指令が発生され
ない状態（即ち、零）においては移動指令がサーボ回路
８ＶＣへ入力され、指令制御が実行され、一方、状態指
令が発生されると。

移動指令と状態指令との合成がサーボ回路ＳＶＣへ入力
され、いわゆる状態適応制御が実行される。

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

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

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

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

〔実施例〕

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

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

図中、第１図で示したものと同一のものは同一の記号で
示してあり、８２は制御部としてのプロセッサ（以下Ｃ
ＰＵと称す）であり、マイクロプロセッサで構成され、
力計測値Ｆｒに所定の利得αを利得手段８２ａで乗算し
２乗算出力α・Ｆｒを入力指令■ｃから合成手段８５で
差し引き指令出力■ｃ′を発するものであり、３は力セ
ンサでアシ。

平行板バネと歪ゲージ３０により構成され、外力によっ
て変位し且つ変位を歪ゲージ３０で検出するもの、２６
はＺ軸モータであり、送りネジ２６ｂを回転させ力セン
サ３に支持されたハンド５をＺ軸方向に移動するもので
あり２位置検出器（エンコーダ）２６ａが設けられたも
の、９は磁気ディスク板（以下円板と称す）であり、ス
ペーサ９０を介在して複数枚積層され、ハンド５によっ
て吸着されるものである。

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

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

図中、第１図及び第２図で示したものと同一のものは同
一の記号で示してあり、８３ｏは速度関数発生部でアリ
、速度指令Ｖｃ’　（δ（ｔ））に対し所定の加速度の
速度関数を発生するものであり、所定の加速度に対応す
る周期のパルスを発振するオシレータ（ＯＳＣという）
　ｓａ０　ａと、ｏｓｃｓａｏａのパルスをアップ又は
ダウンカウントする速度カウンタ８３０ｂと、速度指令
θ（１）と速度カウンタ８３０ｂの内容（追従速度関数
）１；Ｒを比較する比較器８３０　Ｃと、比較器８３０
Ｃの出力によって０８Ｃ８３０ａのパルスを速度カウン
タ８３０ｂのアップ側又はダウン側に与えるスイッチ８
３０ｄとを有するもＯでＳす、θ（ｔ）〉θＲならＯＳ
　Ｃ８３０ａのパルスをアップ側に切換え、速度カウン
タ８３０ｂを加速計数し、δ（ｔ）＝；ｎなら０８　Ｃ
８３０ａ　ノパルスを速度カウンタ８３０ｂへ入力せず
、その内容を保たせ、δ（ｔ）＜δＲならＯＳ　Ｃ８３
０ａのパルスをダウン側に切換え、速度カウンタ８３０
ｂを減速計数させるものである。８３１は速度／位置切
換スイッチであり、後述する比較器の制御（点線）によ
り速度カウンタ８３０ｂと後述する目標位置関数発生部
とを切換えるもの、８３２は比較位置関数発生部であり
、切換スイッチ８３１を介して与えられる２値の速度関
数を積分して比較位置関数θＲ（ｔ）を発生するもので
あり、２値の速度関数とｏ　ｓ　ｃ　ｓａ。

ａのパルス周波数の積を演算して速度関数に応じた周波
数のパルスを出力するバイナリ−レイトマルチプライヤ
（以下ＢＲＭという）　８３２　ａと、ＢＲＭ　８３２
　ａの出力パルスを計数する比較位置カウンタ８３２ｂ
とを有するものである。８３３は比較加速度関数発生部
であり、比較加速度関数を速度カウンタ８３０ｂの加速
、停止又は定速、減速に応じて出力するものであり、ス
イッチ８３０ｄの状態（加速、停止又は定速、減速）に
応じて正値、零。

負値の比較加速度関数θＲ（ｔ）を出力する加速度関数
生成部８３３ａと、比較加速度関数θＲ（ｔ）に対しＪ
／Ａｅ・ＡＭ（但し、Ｊはイナーシャ、Ａｅ　はパワー
アンプ利得、ＡＭはモータの力定数である）を乗じて電
流入力Ｕ　Ｒ（ｔ）を発生する比例乗算部８３３ｂを有
するものである。

８３４は比較器であり、速度カウンタ８３０ｂの値δＲ
と後述する目標位置関数発生部８３５からの減速速度δ
ｄとを比較して一致すると、スイッチ８３１を目標位置
関数発生部８３５側に切換えるためのもの、８３５は目
標位置関数発生部であり、目標である位置指令θ（ｎ）
とカウンタ８３２ｂの比較位置関数θＲ（ｔ）との差に
対応する減速速度信号θｄを発生するものであυ、θ（
ｎ）とθＲ（ｔ）の差をとる差分器８３５ａと、差分器
８３５ａの差に応じた目標位置に位置決めするための減
速信号δｄを発生するリードオンリーメモリ（以下ＲＯ
Ｍと称す）８３５ｂとを有するものである。

８４０は位置カウンタでアわ、モータ２６の位置検出器
（エンコーダ）２６ｂのパルスを計数し現在位置θｐを
表わすもの、８４１は差分器でおり。

比較位置関数θＲ（ｔ）と位置カウンタ８４０の現在位
置θｐとの差をとるもの、８４２はデジタル／アナログ
コンバータ（以下Ｄ／Ａコンハータト称ス）であシ、差
分器の位置（角度）誤差をアナログに変換するもの、８
４３はスムーザであり、エンコーダ２６ｂ角度検知の量
子化誤差によりリミットサイクルの影響を防ぐため、エ
ンコーダ２６ｂのパルス間を補間して位置誤差θｅ（ｔ
）を連続的なものとするためのものであｐ、８４４は加
算器であり、スムーザ８４３の出力をアナログの位置誤
差に加算し連続的な位置誤差θｅ（ｔ）を得るものであ
る。８４５は制御入力発生部であり２位置誤差θｅ（ｔ
）から制御人力Ｕｅ（ｔ）を作成するものであり２位置
誤差θｅ（ｔ）を積分する積分器８４５ａと１位置誤差
θｅ（ｔ）及び制御人力Ｕｅ（ｔ）を入力とし、制御対
象（即ち、パワーアンプ、モータ２６．エンコーダ２６
ｂ）と同一の系を構成して推定角度偏差ｘ＊（ｔ）と推
定角速度偏差ｘ３（１）を発生するオブザーバ（状態観
測器）　８４５　ｂと。

各々積分量ｘ１ｔｔ）　ｌ推定角度偏差Ｘ＊（’）　ｒ
推定角速度偏差Ｘｓ　（ｊ）にフィードバック係数ｆＩ
、ら、ｆ３を乗じる乗算器８４５　Ｃ〜８４５　ｅと１
乗算器８４５Ｃの出力ｆ８・ＸＩ（ｔ）と乗算器８４５
ｄの出力ｆ２・Ｘ２（ｔ）とを負の加算を行う加算器８
４５ｆと、加算器８４５ｆの出力（−ｆｌ　＠　ＸＩｆ
ｔ）　−ｒ、　＠　Ｘ２ｆｔ）　）から乗算器８４５ｅ
（７）出力ｆ３・Ｘ３（ｔ）を減算する減算器８４５ｇ
とを有するものであシ、従って制御人力Ｕｅ（ｔ）は Ｕｅ（’ｌ　＝−ｆｒ　”　ｘ□（ｔ）　　ｒ、　”　
Ｘｚ（ｉ）−ら＊　ｘｓ（ｔ）となる。

８４６は加算器であり、！流入力ＵＲ（ｔ）と制御人力
Ｕｅ（ｊ）を加算し、入力Ｕ　（ｔ）を出力するもの、
８４７はパワーアンプであり、電流駆動形パワーアンプ
で構成され、入力Ｕ　（ｔ）に比例した電流をモータ２
６に供給できるよう内部で電流帰還されているものであ
る。

（ｂ）　　一実施例構成の動作の説明 第３図のサーボ回路ＳＶＣ自体は本発明者等によって提
案され１周知であるが、簡単に第４図を用いて説明する
と、関数発生部８３は、速度（角速度）指令θ（１）と
位置指令θ（１）を受けると、速度関数発生部８３０は
第４図（５）の如くステップ状の速度指令θ（１）に対
し所定の加速度（ＯＳ　Ｃ８３０ａのパルス周波数で決
定される）で追従する速度関数関数発生部８３２で時間
積分され、比較位置関数θＲ（ｔ）が出力される。一方
、比較加速度関数部８３３からは、速度関数δＲ（ｔ）
の加速、減速に応じ比較加速度関数θＲ（ｔ）を発生し
、更にこれによって電流入力ＵＲ（ｔ）を出力する。一
方、目標位置関数発生部８３５は目標位置θ（ｎ）と比
較位置関数θＲ（１）の差に対応する減速信号θｄ（第
４図（５））を発ｄとの値が一致すると（時刻ｔＩ）、
スイッチ８３１を目標位置関数発生部８３５側に切換え
減速信号θｄを比較位置関数発生部８３２に与えてこれ
を時間積分して比較位置関数θＲ（ｔ）を出力せしめる
。従って関数発生部８３は速度指令θ（１）に対し所定
の加速度で追従する速度関数θＲ（りを発生し、この速
度関数に応じて比較加速度関数θＲ（ｔ）を指令人力Ｕ
Ｒ（ｔ）として発生し、且つ速度関数θＲ（ｔ）から比
較位置関数θＲ（ｔ）を作成している。

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

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

ハンド５が円板９に接近中では、状態量としての力セン
サ３の力計測値Ｆｒは零であるから、ＶＣ′＝■であシ
、Ｚ軸モータ２６はサーボ回路８６によってパワーアン
プ８６ａを介し速度制御され、ボールネジ２６ｂを回転
させて、力センサ３を介しハンド５を下降せしめる。

一方、ノ・ンド５が円板９に位置Ｐ、で接触すると。

力センサ３は板バネで構成されているからたわみ（変位
し）、このたわみを検出する歪ゲージ３０より力計測値
Ｆｒが発生する。この力計測値Ｆｒは必ずしもリニアな
特性を有するものではなく、第５図（５）に示す如く、
接触具合によって急激に上昇したシ、振動等によってふ
らつく。

ＣＰＵ８２は１合成部８５よす（■ｃ−α・Ｆｒ）なる
速度指令Ｖｃ’　（ｔ）　（ｔ））を発生し、ＶＣ′は
力計測値Ｆｒに従って減少する。この時、力計測値Ｆｒ
が急激に変化したとすると、速度指令Ｖども急激に変化
し。

その加速度成分である変化率は無限大になるので。

この速度指令にそのままサーボモータ２６を追従させよ
うとすると無限大の電流が要求されることになる。しか
しながら、実際には有限の加速度しか得られないので、
他のサーボ系では追従遅れが出て１位置偏差が大きくな
ｆｉ、Ｄ／Ａコンバータの飽和がおこり、暴走の危険が
あるため、聚急停止となる。（他のサーボの方式でも、
偏差カウンタ方式では偏差カウンタの飽和、ＰＬＬ方式
では税調、パルスモータでの脱調などが生じる。）第３
図のサーボ回路では速度指令ｖＡの指示に対して、一定
の加速度を持ってこれに追従する速度関数を発生するこ
とによシ、力計測値の急激な変化に対して、サーボ系が
常に保護されるようになっている。

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

又、この時、モータ２６はハンド５の円板９への接触に
より太き力性力を受けるが、閉ループ制御部８４の動作
によってこれに打ち勝ち関数発生部８３の速度関数に従
って動作する。

この時、減速の傾きは利得αで制御され、平行板バネが
モータ２６の回転で更にたわみ、力計測値Ｆｒは上昇す
る。最終的には位置Ｐａで実速度■が零となり、この時
、Ｖｃ二α・Ｆｒでおり、札の押し付は力が発生してい
る。

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

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

最適な押し付は力の両方を同時に満たせないおそれがあ
る。

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

この位置Ｐ。においても、サーボ回路ＳＶＣの特性が発
揮される。即ち、正確な力制御のためには。

■ｃ二α・Ｆｒが必要であり、零に近い低速の速度指令
に対しても正確にモータを駆動しなければならない。こ
のサーボ回路ＳＶＣでは関数発生器８３で比較位置関数
θＲ（速度関数θＲの積分値）を作成しているので、微
少な速度指令■ｔでも正確な制御人力Ｕ　（ｔ）を閉ル
ープ制御部８４で作り出せ。

これは比較位置関数θＲに追従するので、まさつ力や外
力に打ち勝った正確かつ精密な制御ができるＯ 従って、接近中は指令速度■によって速度制御され、ハ
ンド５が円板９に接触すると、外力適応制御に自律的に
移行し、最終的な平衡状態では。

時刻ｔ。で位置Ｐ０でＺ軸モータ２６が停止し、実速度
値が零となる。この時入力指令■と力計測値Ｆｒが等し
く、力計測値Ｆｒは力センサ３の変形によって物体９へ
の押し付は方Ｆｏとなるから、入力指令光は力指令とし
て働く。従って、入力指令■の大きさが押し付は力を制
御することになる。

従って、力センサ３のたわみ（変形）により接触時のシ
ョックを吸収しつつ、モータ２６の回転速度をサーボ回
路ＳＶＣによって連続的に落としていき、速度が零とな
った所で、適切な押し付は力を発生するという理想的な
形とすることができる０ このようにして接近、接触、押し付けの３過程が連続的
に円滑に行われ且つ力センサ３の力指令値に急激な変化
が生じても安定に且つ円滑に進行することができる。即
ち、サーボ回路ＳＶＣの閉ループ制御器８４の帰還ルー
プを設けていることにより、まさつ力などのさまざまな
外乱を抑圧して、関数発生部８３の位置の指令（比較位
置関数）に正確に追従する理想的なサーボとなっておシ
。

きわめて精密な速度制御が可能なこと、サーボ系が飽和
しないように関数発生部８３によって加速度成分が一定
値以上にならないよう監視された形で速度をなめらかに
変化させていくことができる。

このサーボ回路は位置指令による位置制御もできるよう
になっており１位置制御が不要であれば。

位置関数発生部８３５．比較器８３４．スイッチ８３１
は不要であり、カウンタ８３０ｂの出力を比較位置関数
発生部８３２に直接入力すればよい。

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

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

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

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

図中、第２図で示したものと同一のものは同一の記号で
示してあり、８２ｂは不感帯部であり。

セットされた不感帯幅Ｗの非線形特性の入出力特性を示
すものである。

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

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

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

入力が正値の時は、（入力−不感帯幅設定値）を出力と
し。

入力が負値の時は、（入力子不感帯幅設定値）を出力と
する。

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

７°。

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

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

この実施例では、第２図の実施例に比し９次の利点が付
加される。即ち、力計測値Ｆｒのフィードバックゲイン
を固定できることから、閉ループ制御系としてのループ
利得を一定に保ったま１．．（つま多制御系としての安
定性を保証しつつ）押し付は力を広い範囲で可変できる
こと、及びハンド５が空中に浮いた状態で（つまシハン
ド５が他の物体を押していない時）、入力Ｖｃをゼロと
し、その場所に止めておきたい時、第２図の実施例では
。

ひずみゲージ３００力計測値にオフセット変動があると
、それにより、ノ・ンド５の位置がドリフトすることが
あるが、この実施例では、不感帯幅より小さなオフセッ
ト変動に対しては、不感になり。

ドリフト現象はなくなる。

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

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

別の実施例として、ＣＰＵ８２内で利得制御及び不感帯
制御の両方を行ってもよく、利得手段８２ａ又は不感帯
部８２ｂｆ：ＣＰＵ８２のプログラムの実行によらず別
のハードウェアで構成してもよい０ （ｄｌ　　別の実施例の構成の説明。

第８図は本発明を直交ロボットに適用した場合の別の実
施例の構成図、第９図は第８図構成の力センサ構成図、
第１０図は第８図構成の合成部。

サーボ制御部及び力制御部のブロック図である。

図中、第１図、第２図、第６図で示したものと同一のも
のは同一の記号で示してあり、ｌａｌ　ｌｂはＸ細雪ジ
ュールでアわ、ロボットのＸ軸位置決め機構を構成し、
各Ｘ軸モータ１０ａ、１０ｂにより搬送パレツ）ｌｌａ
、ｌｌｂをＸ軸方向へ搬送位置決めするもの、２は門型
ロボットであり。

Ｘ軸モジュール１ａ、１ｂの両側に設けられた−対の支
持ベース２０．２１と、Ｙ軸方向に移動するＺ軸ブロッ
ク２２と、Ｚ軸方向に移動するＺ軸可動部（アーム）２
３と、Ｚ軸ブロック２２を送シ、ボールネジ２４ａを回
転させガイド２５ａ。

２５ｂに沿ってＹ軸方向に駆動するＹ軸モータ２４と、
Ｚ軸ブロック２２に設けられ、Ｚ軸可動部２３を図示し
ないボールネジ送り機構を介しＺ軸方向に駆動するＺ軸
モータ２６とを有している。３Ｌ前述の力センサであり
、第９図で示す如く、Ｘ。

Ｙ、Ｚ、　　γの４自由度カセンサで構成されるもの。

４はγ軸モータであり、Ｚ軸アーム２３に支持され、力
センサ３及びハンド５をγ軸を中心に回転させるもの、
５は真空吸着ハンドであシ、筒状本体の先端に吸着面が
設けられるとともに、吸気ポンプに接続された吸気チュ
ーブを有するもの、６は治具であり、バンク）ｌｌａ上
で円板９倉固定するもの、７はベースでオシ、バンク）
ｆｌｂに搭載され１円板９が取付けられるものである０
８０は操作パネルでアシ、オペレータが操作してプレイ
バンクモード、教示モード等を指示するもの、８１はメ
モリであシ、教示データ等を格納するもの、８２はプロ
セッサ（以下ＣＰＵと称す）であり、マイクロプロセッ
サ等で構成され、プレイバック時にメモリ８１の内容を
読出して各部へ指令を与えるもの、ＳＶＣはサーボ制御
部であり。

Ｘ軸モジュール１ａ、１ｂのＸ軸モータ１０ａ。

１０ｂ及びＹ軸モータ２４．Ｚ軸モータ２６及びγ軸モ
ータ４を位置、速度制御するため、ＣＰＵ８２からの指
令位置ｃＸ、ｃｘ、、ＣＹ、ＣＺ、Ｃγが入力され、後
述するハンド位置検出回路からの現在位置ｐｘゎＰＸ、
、ＰＹ、ＰＺ、ＰＹがフィードバックされ、更に合成部
から指示速度Ｖｅｘ〜生部８３と閉ループ制御部８４で
構成されるもの。

８５は前述の合成部であり、第１０図にて後述する如く
、各軸の指令速度ＶＸ１〜■ｒと後述する力制御部の力
制御出力ＰＦ、−ＰＦ、との差をとり、サーボ制御部Ｓ
ＶＣの関数発生部８３へ与えるもの。

８７は力制御部であり、第１０図にて後述する様に力セ
ンサ３の検出出力Ｐ、−Ｆ、を受け、これをデジタル値
Ｐ、−Ｆ、に変換するとともに不感帯を設定して制御出
力ＰＦ、−ＰＦ、を出力するもの、８８はハンド位置検
出部（閉ループ制御部８４のカウンタ８４０）テロす、
各軸のモー１１０ａ、１０ｂ。

２４．２６．４に設けられたロータリーエンコーダ２６
ｂ等の出力から各軸の現在位置ＰＸ１．ＰＸ２゜ＰＹ、
ＰＺを求め、ハンド５の現在位置を得るもの、８９はバ
スであシ、ＣＰＵ８２とメモリ８１゜操作パネル８０．
サーボ制御部８３１合成部８５゜力制御部８７及びハン
ド位置検出回路８８とを接続し、データ、コマンドのや
シとりを行なうものである。

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

力センサ３は、ｘ、ｙ、ｚ軸の外力を検…するＸ、Ｙ、
Ｚ力検出モジュール７４０と、γ軸の外力を検出するγ
力検出モジュール７５０とで構成される０ 力検出モジュール７４０は、第９図から明らかな如く、
各平行板バネ体が変位方向が互いに直交するように設け
られているので、平行板バネ体ａｌ。

ａ　１’でＸ軸方向のたわみ、平行板バネ体ｂ１．ｂ１
′でＹ軸方向のたわみ、平行板バネ体ｃｌ、ｃｌ’でＺ
方向のたわみを夫々分担する３自由度を有する０ ７４３　、７４４は夫々力検出モジュール７４０を支持
する支持体であって、支持体７４３はねじ７４５により
角棒７４２と連結され、支持体７４４はねじ７４６によ
り角棒７４１と連結されている。尚、ねじ７４５゜７４
６は片方のみ示し、さらに、各ねじ７４５が螺合するね
じ穴７４３ａと他方の穴は中心穴７４０ａの中心位置か
ら等しい距離の位置に設定され、同様にねじ７４６が螺
合するねじ穴７４４ａと７４４ｂは中心穴２５０ａの中
心位置から等しい距離（Ｌ９＝Ｌ１０）の位置に設定さ
れている。

７４７は支持体７４３にねじ７４８によ多連結される出
力棒であって、力検出モジュール７４０に設けられた穴
７４０ａを貫通するように構成されている。

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

尚、出力棒７４７は力検出モジュール７４０に設けられ
た穴７４０ａを貫通するよう構成されているが。

支持体７４４を貫通するように構成してもよく、この場
合は、支持体７４４の基台への取付けを反対側（角棒７
４１　、７４２側）で連結する必要がある。

７４９ａ、　７４９ｂ、　７４９ｃ、　７４９ｄ、　７
４９ｅ、　７４９　ｆは歪ゲージであって、夫々各平行
板バネ体ａｌ’、ｂｌ。

Ｃ１′の変位を検出する。ここで、・この歪ゲージは軸
方向の力をトルクの影響を受けずに検出するため、中心
穴７４０ａを中心として中心点対象となるように貼付し
、夫々ブリッジ回路を構成せしめる。

従って９図示されていないが、平行板バネ体ａ１、Ｃ１
１ｂｌ’にも歪ゲージが中心穴７４０ａの中心点対象位
置となるように各々２枚づつ貼付されている。

以上説明した構成とすることにより９例えば。

出力棒７４７はＸ軸方向の力が加わった場合、歪ゲージ
７４９　Ｃ、７４９ｄが平行板バネ　ｉ／の変位を検出
し、Ｘ軸方向のみの力を検出でき、同様にＸ軸方向の力
が加わった場合歪ゲージ７４９　ｅ　、　７４９　ｆが
平行板バネｂ１の変位を検出し、Ｚ軸方向の力が加わっ
た場合歪ゲージ７４９　ａ　、　７４９　ｂが平行板バ
ネａ１′の変位を検出し、各軸の力成分を検出する。

さらに複数方向の合力が加わった場合でも、角棒７４１
　、７４２に加わる力の位置は中心穴７４０ａの中心位
置から等しい距離の位置に支持体７４３，７４４により
加わるため、各平行板バネ体が夫々の分力ｒｘ、ｉｔ’
ｙを独立して検出することができる。

７５０はγ力検出モジュールであって、力検出モジュー
ル７４０の出力棒７４７にねじ７５１を介して取付けら
れる中心部材７５２を備えると共に、板バネ７５０　ａ
、　７５０　ｂ、　７５０　Ｃ，７５０ｄ　　を介して
接続される外輪７５３を含む。７５４ａ、　７５４ｂ、
　７５４ｃ、　７５４ｄ　　は歪ゲージであって、板バ
ネ７５０　ａ　、　７５０　Ｃに貼付（中心部材７５２
の中心点対象位置で、同一面側）され、同様にブリッジ
回路を構成する。

尚、γ力検出モジュール７５０の出力棒７４７への取付
けは、ねじ７５１のみで出力棒７４７の中心位置として
いるが、この構成では外輪７５３にトルクを与えた際に
、ねじ７５１のゆるみ等が生じるため。

実際には、中心部材７５２から突出するピンを出力軸７
５７に係合させてまわり止めを施すと共に、中心位置か
らずれたところでねじ７５１により固定する必要がある
。

また、このことは、出力軸７５７と支持体７５４との結
合の場合も同様である。

この構成とすることによシ中心部材７５２を固定し、外
輪７５３に中心軸（γ軸）まわりのトルクを加えると板
バネ７５０ａ、　７５０ｂ、　７５０Ｃ，７５０ｄ　　
がたわム。コノたわみを歪ゲージ７５４　ａ、　７５４
　ｂ、　７５４　Ｃ。

７５４ｄで検出し、ブリッジ回路を介して出力を取り出
すことにより、２軸（γ）に関するトルクＴ。

のみを検出することができる。

第１０図は力制御部８７２合成部８５．サーボ制御部Ｓ
ＶＣの詳細回路図である。

図中、第８図で示したものと同一のものは同一の記号で
示してあ、１．ｓｏｏ〜８０３は各軸の合成回路であり
、力制御部８７から力制御指令ＰＦＸ−ＰＦ、とＣＰＵ
８２からの速度指令Ｖｘ〜Ｖｚとの差を出力するもの、
８０４〜８０７は不感帯部であり、ＣＰＵ８２からの不
感帯幅Ｗ、−Ｗ、が設定され、力計測値Ｐ、−Ｆ、に対
し不感帯を与えるもの、８０８はスイッチであシ、力計
測値Ｐｘ　−Ｆｒの入力の許可／不許可をするもので１
Ｌカフイードバツクオン／オフをｃｐｕｓｇの制御によ
り行うもの、８０９はアナロク／テシタルコンバータ（
Ａ／Ｄコンバータと称す）であり、力センサ３からのア
ナログの力計測値ＦＸ、ＦＹ、ＦＺ、ＦＴをデジタル値
に変換して、スイッチ８０８に出力するもの、８３ａ〜
８３ｄは各々各軸の関数発生部で１ｈ指令位置ＣＸ、Ｃ
Ｙ、ＣＺ、Ｃγと合成指令速度■〜Ｗが入力されるもの
、８４ａ〜８４ｄは各々各軸の閉ループ制御部であり各
軸のモータを駆動するもの。

８８は前述の位置検出器であ＃）、各軸モータに設けら
れたエンコーダの出力を計数し、各軸の現在位置を検出
するものである。

従って、ＣＰＵ８２によってスイッチ部８０８がオフの
時には、力センサ３の出力（即ちＡ／Ｄコンバータ８０
９の出力）は不感帯部８０４　、８０５　。

８０６　、８０７へ入力されず、カフィードバックオフ
となり、ＣＰＵ８２からの指令位置ＣＸ、ＣＹ。

ＣＺ、Ｃγ及び指令速度ｖｘ、ｖｙ、ｖｚ、ｖγがその
まま各軸の関数発生部８３ａ〜８３ｄに入力され１位置
、速度制御され、指令制御モードが実行される。

一方、ＣＰＵ８２によってスイッチ部８０８がオンの時
には、カフィードバックオンとなり、力センサ３の出力
は不感帯部８０４〜８０７を介し制御出力ＰＦＸ、ＰＦ
Ｙ、ＰＦＺ、ＰＦγとなって合成部８００〜８０３に入
力し、指令速度ｖｘ、ｖｙ、ｖＺ、ｖγとの合成出力Ｖ
’Ｘ、　Ｖ’Ｙ、　Ｖ’Ｚ、　’ｉ’ｒが関数発生部８
３８〜８３ｄに速度指令として寿えられ、適応制御モー
ドが実行される。

尚、Ｘ軸はＸｉ軸とＸ２軸の２つがあるが、１つのＸ軸
関数発生部Ｓａａ、閉ループ制御部８４ａで示してあり
実際には２つある。

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

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

■先づ、ＣＰＵ８２は円板９のＸ、Ｙ座標位置及び所定
のＺ座標位置として位置指令Ｃｘ、、　ｃｙ。

ＣＺをバス８９を介し、関数発生部８３へ与え且つ合成
部８５にｘ、ｙ、ｚ速度指令ＶＸＩ、ＶＹ。

■Ｚを与える。

これによって、関数発生部８３より閉ループ制御部８４
を介し駆動電流ｓｘ、、ｓｙ、ｓｚがＸ軸モータ１０ａ
ｌＹ軸モータ２４．Ｚ軸−Ｆ−−夕２６に供給される。

これによって、Ｘ軸モジュール１ａのＸ軸モータｌＱａ
、Ｙ軸モータ２４．Ｚ軸モータ２６が駆動されて、真空
吸着ハンド５はＸ軸モジュールｌａ上のパレット１１ａ
の治具６の円板９上の所定のＺ位置（第２図の点Ｐ）に
Ｘ−Ｙ位置決めされる。

■次に、ＣＰＵ８２はハンド位置検出回路８８の各軸の
現在位置ＰＸ２．ＰＹ、ＰＺからの所定のハンド位置に
到達したか否かを調べ、そして所定の位置に停止した後
の所定の時間経過後（０，５秒程度）、力センサ３の振
動停止とみなし、カフイ■ＣＰＵ８２は、バス８９を介
し速度指令値■Ｚを合成部８５へ与える。前述の如く１
円板９への接近中はＰＦ’Ｚ＝Ｏであるから、関数発生
部８３Ｃへは指令速度として出力され、Ｚ軸モータ２６
を速度制御する。従って、吸着ハンド５は円板９に向っ
て指令速度■１で下降する。

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

吸着ハンド５０円板９への押し付は力が発生し。

徐々に押し付は力は増加する。

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

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

更にＣＰＵ８２は、吸着ハンド５を上昇すべく。

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

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

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

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

第１２図はＣＰＵ８２の処理説明図である。

この例では８合成部８５及び力制御部８７０機能をＣＰ
Ｕ８２のプログラムの実行によって行なうものである。

ＣＰＵ８２はメインルーチンにおいて、教示データのコ
マンドを解析し、これを実行して各軸の位置指令、速度
指令、力指令を作成し、バス８９を介し関数発生部８３
へ出力し、又位置検出器８８からの現在位置Ｐ、−Ｐ、
、によって各軸の位置及び力センサ３からの力計測値Ｐ
、−Ｆ、を監視する。そして１通常（指令）モードでは
、スイッチの点線の如く指令位置と指令速度をそのまま
関数発生部８３へ与えて、閉ループ制御部８４を介し各
軸を指令位置に位置決めする。一方、カフィードバック
モードにおいては、所定周期でカフィードバック制御の
割込み処理ルーチンを実行する。即ち。

力センサの力計測値Ｆ、−ｐｘをオフセット補正し。

さらに不感帯処理して、装置利得を掛は制御出力を得、
これを指令速度又は指令力から差し引いたものを指令速
度Ｖ；　−ＶＱ、とじてスイッチ実線の如くバス８９を
介し関数発生部８３へ与える。

この不感帯処理においては、不感帯幅Ｗｒ−Ｗｘを自由
に設定でき、セットされた不感帯幅がＷである時は、力
計測値Ｆが正なら（Ｆ−Ｗ）を出力し。

負なら（Ｆ＋Ｗ）を出力するものである。

とのカフィードバック制御によって前述の如く次の様な
制御ができる。

ある軸（例えばＺ軸）に指令速度Ｖｚを与えると。

物品への接近中は、指令速度ＶＺで移動する。一方。

物品に点Ｐ１で接触すると、力センサがたわんで変形し
、変形量に応じた力計側値Ｆｚが発生するが不感帯幅Ｗ
の範囲では制御出力Ｐｚは零のため、速度はＶｚのまま
となり、　Ｆｚ＞Ｗとなると制御出力Ｐｚが発生し、指
令速度Ｖｚを減少していく、従ってＺ軸の移動速度も減
少し、　Ｐｚ＝Ｖｚとなると、Ｚ軸は停止する。この時
Ｐｚの押し付は力が物品へ付与されており、従って指令
速度Ｖｚは指令力となっている。

これによって物品への接近、接触、押し付けが連続的に
実行される。

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

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

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

以上本発明を実施例によし説明したが１本発明は本発明
の主旨に従い種々の変形が可能でおり。

本発明からこれらを排除するものでは々い。

〔発明の効果〕

以上説明した様に２本発明によれば、移動指令と状態指
令とを合成して速度指令としているので円滑に且つ自律
的に状態適応制御が実行することができるという効果を
奏し、従って高速動作も可能となる。

又、外乱である状態指令に対しサーボ回路が安定に且つ
円滑に速度制御することができるという効果を奏し、係
る状態フィードバックを行っても状態適応制御を安定に
且つ円滑に行なうことができる。

【図面の簡単な説明】

第１図は本発明の原理説明図。 第２図は本発明の一実施例構成図。 第３図は第２図構成のサーボ回路の回路図。 第４図は第３図サーボ回路の特性図。 第５図は第２図構成の動作説明図。 第６図は本発明の他の実施例構成図。 第７図は第６図構成の動作説明図。 第８図は本発明の別の実施例構成図。 第９図は第８図構成の力センサ構成図。 第１０図は第８図構成の合成部及び力制御部ブロック図
。 第１１図は第８図構成における吸着動作処理フロー図。 第１２図は本発明の更に別の実施例説明図であるＯ 図中、３・・・力センサ（状態検出手段）。 ５・・・ハンド（移動体）。 ９・・・円板（物体）。 ２６・・・Ｚ軸モータ（駆動手段）。 ８２・・・制御部。 ８５・・・合成部。 ８ＶＣ・・・サーボ回路。 ８３・・・関数発生部。 ８４・・・閉ループ制御部。 ”ＡＷｉＥω、ｌ＜＼／

Claims (3)

【特許請求の範囲】
1. （１）移動体（５）を駆動する駆動手段（ＭＴ）と、該
   移動体（５）の物体に対する状態を検出する状態検出手
   段（３）と、 該駆動手段（ＭＴ）を速度制御するためのサーボ回路（
   ＳＶＣ）と、 指令値（Ｖｃ）と該状態検出手段（３）の出力（Ｆ）と
   を合成した信号（Ｖｃ′）を該サーボ回路（ＳＶＣ）に
   供給する制御手段（８２）とを備え、 該サーボ回路は、該合成した信号に対し所定の加速度を
   もつて追従する速度関数を発生する関数発生部（８３）
   と、 該速度関数を積分した比較位置関数と該駆動手段の検出
   位置とを比較して該駆動手段に制御入力を与える閉ルー
   プ制御部（８４）とを有することを特徴とする移動体制
   御装置。
2. （２）前記状態検出手段が、接触型検出手段であること
   を特徴とする特許請求の範囲第（１）項記載の移動体制
   御装置。
3. （３）前記接触型検出手段が、外力によつて変位し且つ
   変位を検出する力検出手段であることを特徴とする特許
   請求の範囲第（２）項記載の移動体制御装置。