JPH0380310A - マニピュレータ制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 マニピュレータ制御装置の改良に関し、マニピュレータ
の共振状態を十分に除去できるようにすることを目的と
し、 ノッチ・フィルタ演算部からの速度指令信号に基ツいて
マニピュレータを作動させる関節サーボ部と、マニピュ
レータが受ける力を検出する力検出器と、マニピュレー
タの現位置を検出する位置検出部と、検出された力及び
位置並びに目標とする力及び位置に基づいて速度指令信
号を発生する力＆位置制御演算部と、力＆位置制御演算
部からの速度指令信号が入力されるノッチ・フィルタ演
算部と、力＆位置制御演算部から出力される速度指令信
号系列に対してＦＦＴ演算を行うＦＦＴ演算部と、ＦＦ
Ｔ演算の結果に基づいて速度指令信号の主周波数成分を
検出し当該主周波数成分がマニピュレータが追従可能な
低域周波数を外れた場合には当該主周波数成分を減衰さ
せるノッチ・フィルタの係数を算出する定数算出部とを
有することを構成としている。

〔産業上の利用分野〕

本発明は、速度指令信号に基づいてマニピュレータを作
動させる操作部と、マニピュレータが受ける力を検出す
る力検出器と、マニピュレータの現位置を検出する位置
検出部と、検出された力及び位置並びに目標とする力及
び位置に基づいて速度指令信号を発生する力＆位置制御
演算部とを備えたマニピュレータ制御装置の改良に関す
る。

〔従来の技術〕

従来、この種の制御装置として、対象物の表面に沿って
加工、パリ取り、拭き取り等の倣い動作をするハンド部
を備えたマニピュレータに用いる制御装置がある。

第８図は従来の制御装置の例を示すブロック図である。

同図において、１はコントローラ、２は力検出器、３は
パワー・アンプ、４はマニピュレータ、５はエンコーダ
、６は力９位置指令値、９はホスト・コンピュータ、１
０は関節サーボ演算部、１１は位置算出部、１２は力検
出演算部、１３は力＆位置制御演算部、１４はノッチ・
フィルタ演算部、１７は力覚センサをそれぞれ示してい
る。

制御装置は、速度指令信号に基づいてマニピュレータ４
の操作を行う関節サーボ演算部１０と、マニピュレータ
４のハンド部（図示せず）の現在位置を検出する位置算
出部１１と、ハンド部が受ける力の検出を行う力検出器
２と、力＆位置制御演算部１３とを有している。位置算
出部１１は、エンコーダ５から出力されたパルスをカウ
ンタすることによりマニピュレータの関節角を求め、こ
の関節角を基準座標系上の位置に変換するものである。

力＆位置制御演算部■３は、力制御演算部と位置制御演
算部を有しており、位置制御演算部は位置算出部１１が
算出した位置Ｐｇ、マニピュレータ４の目標位置Ｐ０及
び対象物の形状などによって定められた位置パラメータ
に基づいてマニピュレータ４の位置制御方向（移動方向
）の速度指令信号を発し、力制御演算部は力検出演算部
１２が検出した力Ｆ８．目標とするハンド部の押付力Ｆ
ｏ　（以下、設定力と言う）及び対象物の形状などによ
って定められた力制御パラメータに基づいて速度指令信
号を発する。位置制御演算部から出力される速度指令信
号と力制御演算部から出力される速度指令信号の和が、
力＆位置制御演算部１３から出力される速度指令信号Ｕ
となる。上記力検出器２は、力覚センサ１７と力検出演
算部１２とを備えている。この力覚センサ１７は、例え
ば外力によって撓む弾性梁の歪を歪ゲージで検出するも
ので、マニピュレータ４の手首部とハンド部との間に装
着されたおり、ハンド部の一作用点にかかる力及びモー
メントを検出するものである力検出演算部１２は、ハン
ド座標系上の力をロボット基準座標系上の力に変換する
ものである。

位置＆力制御演算部１３から関節サーボ演算部１０へ速
度指令信号が発信されると、関節サーボ演算部１０は速
度指令信号に基づいて目標位置を更新し、目標位置と現
在位置の差に応じた操作量Ｖをパワー・アンプ３に与え
、パワー・アンプ３の出力がマニピュレータ４のモータ
に与えられる。

そして、常に上記設定力Ｆ０がハンド部に与えられるよ
うに、力検出器２でハンド部に作用する力を検出し、こ
の検出に基づいて力＆位置制御演算部１３から対応した
速度指令信号を送出（フィードバック制御）している。

ところで、上記力検出器２においては、対象物に対する
ハンド部の押付力だけではなく、ハンド部の重さや慣性
力、対象物とのｗｅ力、マニピュレータを伝播する振動
（例えばマニピュレータ４の減速器が発生する振動）に
起因するノイズも検出してしまう。特に、マニピュレー
タ４を伝播してきた振動は、マニピュレータの固有振動
数を含んでいるので、上記フィードバック制御を行うと
、固有振動数に対応した速度指令信号が発信され、その
ためマニピュレータ４が直ちに共振してしまう。

そこで、共振を防ぐための有効な手段として、この固有
周波数に関する速度指令信号を与えないために、ノッチ
・フィルタ１４によって速度指令信号から固有周波数成
分を除去する方法がある。

従来においては、予め振動解析で求めておいたマニピュ
レータの固有振動数をノッチ・フィルタ１４のノツチ底
周波数ｆｃ　（第３図参照）に設定している。即ち、ホ
スト・コンピュータ９から予め設定した特性のフィルタ
・パラメータａ、、ａ。

ａ２がコントローラ１に転送されている。

〔発明が解決しようとする課題〕 ノッチ・フィルタは、連続系の伝達関数Ｇ　（ｓ）が以
下で与えられるものである。

なお、ωｃ−２πｆＣであり、Ｓはラプラス演算子であ
る。また、ζは減衰係数であり、ｆｃが除去したい周波
数（ノツチ底周波数とする）である。

伝達関数Ｇ　（Ｓ）のボード線図は第３図に示すように
なり、ｆＣが除去したい周波数から外れると、除去効果
が得られないことがわかる。従来の制御装置においては
、固有振動数を予めサーボ解析等によって求めておき、
これをｆｃとするのであるが、マニピュレータの使用状
態によって必ずしも固有振動数がその値と一致しない。

即ち、ハンドを取り替えることによるイナーシャの変化
や長期使用による閏ｉｆｆ剛性の変化（ギヤ与圧やギヤ
の摩耗などによる）、更には多関節型マニピュレータに
おいては姿勢変化と言ったことによって、マニピュレー
タの固有振動数が解析結果と一致しない場合があり、且
つ一定値でない場合がある。このような場合、速度指令
信号の中の固有振動数成分をカットすることができない
ので、マニピュレータの共振状態を充分に除去できない
と言う問題があった。

本発明は、この点に鑑みて創作されたものであって、マ
ニピュレータの固有振動数の推定のずれ又は変動が生じ
ても、速度指令信号の中の固有振動数成分をカットし、
マニピュレータの共振状態を十分に除去できるようにな
ったマニピヱレータ制御装置を提供することを目的とし
ている。

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

第１図（ａ）は本発明の請求項（１）の発明を示す図で
ある。請求項（１）のマニピュレータ制御装置は、マニ
ピュレータ４と、マニピュレータ４が受ける力を検出す
る力検出器２と、マニピュレータ４の現在位置を検出す
る位置検出部２１と、これら検出された力及び位置並び
に目標とする力及び位置に基づいて速度指令信号を発す
る力＆位置制御演算部１３と、力＆位置制御演算部１３
から出力される速度指令信号の時系列を格納するメモリ
部７と、上記速度指令信号に対して高速フーリエ変換演
算を行う高速フーリエ変換演算部１５と、高速フーリエ
変換の結果得られる周波数スペクトルのピーク周波数を
検出し当該周波数がマニピュレータ４が追従可能な低域
周波数を外れたものである場合に当該周波数成分を減衰
させるノッチ・フィルタの離散形演算式の係数を算出す
る定数算出部１６と、力＆位置制御演算部１３の出力す
る速度指令信号の中から上記係数で指定される周波数成
分を減衰させるノッチ・フィルタ演算部１４と、ノッチ
・フィルタ演算部１４から出力される速度指令信号に基
づいてマニピュレータ４を作動させる関節サーボ部２０
とを具備している。

第１図ｃｂ＞は本発明の請求項（２）の発明を示す図で
ある。請求項（２）の発明は、請求項（１）の発明に長
さ設定部１８を追加したものである。

〔作用〕

第１図（ａ）において、力検出器２はマニピュレータ４
に働く力を検出し、位置検出部２１はマニピュレータ４
の先端位置を検出する。力＆位置制御演算部１３は、検
出された力および位置並びに目標とする力及び位置に基
づいて速度指令信号を出力する。この速度指令信号は、
メモリ部７に格納されると共に、ノッチ・フィルタ演算
部１４に送られる。メモリ部７に、Ｎ個の速度指令信号
が格納されると、ＦＦＴ（高速フーリエ変換）演算部１
５は、メモリ部７の速度指令信号列をもとにＦＦＴ演算
を行う、定数算出部１６は、高速フーリエ変換の結果得
られる周波数スペクトルのピーク周波数を検出し、当該
周波数がマニピュレータ４が追従可能な低域周波数を外
れたものである場合に当該周波数成分を減衰させるノッ
チ・フィルタの離散形演算式の係数を算出し、算出した
係数をノッチ・フィルタ演算部１４に送る。ノッチ・フ
ィルタ演算部１４は、力＆位置制御演算部１３から出力
される速度指令信号の中から上記係数で指定される特定
周波数成分を除去する。ノッチ・フィルタ演算部１４か
ら出力される特定周波数成分が除去された速度指令信号
は関節サーボ部２０に入力され、この値に応じてマニピ
ュレータ４のモータがサーボ制御される。

第１図ｒｂ＞においては、メモリ部７に格納される速度
指令信号の個数Ｎは長さ設定部１８によって決定される
。長さ設定部１８は、設定したノッチ・フィルタ演算部
１４によって所望の減衰が得られる周波数幅を周波数精
度とした高速フーリエ変換演算が行なえるように、標本
値系列（速度指令信号系列）の長さＮを決定する。

〔実施例〕

第２図は本発明の１実施例の機能ブロック図である。同
図において、１はコントローラ、２は力検出器、３はパ
ワー・アンプ、４はマニピュレータ、５はエンコーダ、
６は力２位置指令値、７はデータ・メモリ・ボード、８
はフィルタ設定用プロセッサ、１０は関節サーボ演算部
、１１は位置算出部、１２は力検出演算部、１３は力＆
位置制御演算部、１４はノッチ・フィルタ演算部、１５
はＦＦＴ演算部（高速フーリエ変換演算部）、１６は定
数算出部、１７は力覚センサ、１８はＮ設定部をそれぞ
れ示している。

第２図に示すように、本発明の実施例においては、第８
図の従来例にデータ・メモリ・ボード７とフィルタ設定
用プロセッサ８が追加されている。

ファイル設定用プロセッサ８は、ＦＦＴ演算部１５と、
定数算出部１６と、Ｎ設定部１８とを有している。

データ・メモリ・ボード７は速度指令信号の時系列ｕ０
．・・・ｕＮ−１を格納するものである。ＦＦＴ演算部
１５は、データ・メモリ・ボード７に格納された速度指
令信号の時系列ｕｏ＋　・・・ｕ）ｉ−１４もとにＦＦ
Ｔ演箕を行う。定数算出部１６は、ＦＦＴ演算結果に基
づいて動作時の速度指令信号Ｃ主周波数成分を検出し、
その主周波数成分がマニピュレータの追従可能な低域周
波数を外れたもＣであった場合、その主周波数成分を減
衰させるノッチ・フィルタのパラメータａｎ　＋　　ａ
ｔ　ｒ　　ａｔ　４算出する。Ｎ設定部１８は、データ
・メモリ・４−ドアに格納される速度指令信号の標本値
系列Ｃ長さを決定する。

第２図において、サーボ演算部１０よりマニピュレータ
４が作動させられると、力検出演算部１２によりマニピ
ュレータ４が現在受けているカカ検出され、位置算出部
１１によりマニピュレータ４の現在位置が検出される。

力＆位置制御演算＠□１３は、これら検出された力及び
位置並びに自社とする力及び位置に基づいて速度指令信
号を発づる。この力＆位置制御演算部１３におけるアル
：リズムは、力と位置のハイブリッド制御もしり番；コ
ンプライアンス制御として公知である。

力＆位置制御演算部１３から出力された速度指令信号は
、ノッチ・フィルタ演算部１４とデータ・メモリ・ボー
ド７へ送られる。データ・メモリ・ボード７は、ＦＦＴ
演算に必要なＮ個（後述する）の標本値データをサンプ
リング順に格納し、Ｎ個目を格納した時点でＳＥＴフラ
グを立てると同時に、以後のデータはＦＦＴ演算部１５
におけるＦＦＴ演算の終了を告げるＲＥＳＥＴフラグが
立つまで受は付けない。ＦＦＴ演算によって入力信号列
の周波数スペクトルが得られ、その最大値を示す周波数
ｆ！を上記速度指令信号の現時点での主周波数と見做す
。

ところで、ｒえは必ずしもマニピュレータの固有振動数
ではないが、速度指令信号として実際にマニピュレータ
が追従できる帯域は低く　（せいぜい２０Ｈｚ）、その
最大値をｆ、とすれば、ｆ。

以上の周波数成分を速度指令信号から除去しても差し支
えない。

前記速度指令信号の標本値系列の長さＮ（即ち、メモリ
に格納する個数）は、以下の要領でＮ設定部１８で決定
される。

ノツチ底周波数ω。に設定したノッチ・フィルタにスレ
ッシュホールド・ゲインＣｔｈとなる周波数は、第４図
に示すように、 ω／ω、−ｆ／ｆｃ＝Ｘ、、ｘｚ であり、フィルタ入力の主周波数成分ｆ、がｘ＋　＜ｆ
ｉ　／ｆｃ＜Ｘｔ ・・・（２） であれば、ｆｉの成分はノツチ底周波数をｒｃに設定し
たノッチ・フィルタによって十分に減衰されていると見
做す。

ここで、ＦＦＴによって周波数分析したときの周波数精
度Δｆは、第５図に示すように、ｆｃに対して、 ａｔ　−（ｘｚ　−１）　ｆｃ−（３）で与えることに
する。一方、前記標本値系列のサンプリング周期を１１
とすると、ΔｒとＮとの間に次式が成立する。

Δｆ−ｔ、／Ｎ　　　　　　　　　　　　・・・（４）
したがって、（３）、　（４）式より、Ｎ−ｆ　−／　
ＣＸｔ　　　１　）　ｆ　ｃ・・・（５）となる、（５
）式においてＬ　＝ｆ、としたときのＮが十分な値であ
り、即ち、 Ｎ＝ｆ−／　（Ｘｉ　　　１）ｆｔ　　　　　　　”・
（６）となる。

定数算出部１６では、最も大きいスペクトルを示す周波
数ｒ長を検出して、ｆ、がｆＬ以上であり且つ既設のフ
ィルタ周波数ｒｃと異なる場合に、ｆｌの値をｆ＋の値
に更新し、ノッチ・フィルタ演算部１４で実行するパラ
メータａＯ＋　　ａｔ　１　８２を次の（８）式によっ
て算出する。

（１）式をディジタル化するために、双線形変換を施す
と、以下の弐が得られる。

Ｙｋ−ａｓ　ｕｍ　＋ａ＋　ｕｍ−＋　＋ａＯｕｋ−２
−ａ、Ｙ、、＋ａｔＹｋ−。

・・・（７） ここで、Ｙｌｌ、　Ｙｍ−＋　、　Ｙｋ−□はフィルタ
の出力、ｕｋ ｕｋ ｍ−ｚ は入力である。

また、 ｎ （ご。

＋４） α ａ　＋　　−（”ｉ；ｆ”ｅ　”　　　４　）　　　　
　　　　＝（８）α ３、＝　　　　　（４４ζγ。十１２）α である。ここで、 α＝４＋４ζωゎ＋ωｃ２ ω、−２πｒｃ ｔ−ωｃＴ であり、Ｔはサンプル周期である。

ノッチ・フィルタ演算部１４は、（７）式を実行する。

なお、添字に、に−１，に−２はそれぞれ現時点、一つ
前、２つ前のＵおよびＹを示す。

以上のフィルタにより、マニピュレータの共振状態を十
分に除去できる。

第６図は本発明を実施するための装置のハードウェア構
成例を示す図である。同図において、１はコントローラ
、２は力検出器、３はパワー・アンプ、４はマニピュレ
ータ、５はエンコーダ、６はホスト・コンピュータ、７
はデータ・メモリ・ボード、８はフィルタ設定用プロセ
ッサをそれぞれ示している。

コントローラ１は、ＣＰＵ、メモリ及びＩｌｏで構成さ
れ、力＆位置制御演算部１３．関節サーボ演算部１０お
よυノッチ・フィルタ演算部１４の各演算を行う。力検
出器２は、力覚センサ１７および力検出演算部１２で構
成され、検出値はＩｌｏを通してコントローラ１に取り
込まれる。

マニピュレータ４は、コントローラｌのＩｌｏより出力
された指令電圧を受けたパワー・アンプ３によって駆動
される。また、マニピュレータ４の関節変位は、エンコ
ーダ５の出力を■／○を通してコントローラ１に取り込
むことによって検出され、位置算出部１１によってマニ
ピュレータ４（即ち、ハンドの先端）の位置に換算され
る。

ホスト・コンピュータ６は、力と位置の目標値を生威し
、バスを通してコントローラｌに送る。

データ・メモリ・ボード７は、メモリとカウンタで構成
され、後述するＲＥＳＥＴフラグがバスから送られて来
ると、カウンタがクリアされ、コントローラ１からの速
度指令信号をメモリに格納する。そして、データがＮ個
（即ち、カウンタの値がＮ）になった時にＳＥＴフラグ
を立て、且つコントローラからの速度指令信号を受は付
けない。

フィルタ設定用プロセッサ８は、ＣＰＵおよびメモリで
構成され、ＦＦＴ演算部１５．定数算出部１６およびＮ
設定部１８の演算を行う。

Ｎ設定部１８は、コントローラ１のサーボ周期をｒｓと
したとき、ｆｔおよびζを定数として（５）弐よりＮを
算出し、データ・メモリ・ボード７にフルカウント値の
参照値を与え、ＦＦＴ演算部１５に標本値系列の長さＮ
を与える。

ＦＦＴ演算部１５は、ＳＥＴフラグを確認した後、デー
タ・メモリ・ボード７のメモリをアクセスしてＮ個の速
度指令信号列を取り込み、ＦＦＴ演算を行い、周波数ス
ペクトルを算出する。また、ＦＦＴ演算終了とともにＳ
ＥＴフラグを立てる。

定数算出部１６は、前記周波数スペクトルのピ一りとな
る周波数ｆｉを検出し、ノッチ・フィルタのノツチ底周
波数ｆｃを変更するか否かを判定する。即ち、 ｆｉ＞ｆｔ且つｆｔ　≠ｆｃ であれば、ｆｃの値をｆｉの値に変更し、前記（８）式
によって、コントローラ１のフィルタ演算部１４で参照
されるパラメータａＯｒ　　ａ　）　ｒ　　ａ　ｚを求
め、コントローラ１のメモリにバスを通じて転送する。

第７図はフィルタ設定用プロセッサの処理フローを示す
。

■　Ｎ−ｆ、／　（Ｘ、−１）ｆ、の計算を行う。

■　ＳＥＴフラグがオンか否かを調べる。Ｙｅｓのとき
は■に進む。

■　メモリをアクセスし、ＵＯ＋　・・・、Ｕ□１に対
してＦＦＴ演算を行う。

■　データ・メモリ・ボード７ヘリセツト信号を送る。

■　ピーク周波数ｆ正を検出する。

■　（ｆｔ　＞ｆｃ　）　ｎ　（ｆｔ　”ｆ−）か否か
を調ベる。Ｙｅｓのときは■に進む。

■　ｆｃ＝ｆ□とする。

■　ａ６．ａｌ、ａｚを算出する。

■　ａ６　＋　　ａｌ　、ａｚをコントローラ１のメモ
リに転送する。

〔発明の効果〕

以上の説明から明らかなように、本発明によれば、マニ
ピュレータの固有振動数の推定ずれや変動が生じても、
マニピュレータの固有振動数に関する速度指令信号をカ
ットし、マニピュレータの共振状態を常に十分に除去す
ることが出来る。

【図面の簡単な説明】

第１図は本発明の原理説明図、第２図は本発明の１実施
例の機能ブロック図、第３図はノッチ・フィルタのボー
ド線図例を示す図、第４図はゲインがＧいとなる周波数
を示す図、第５図はＦＦＴによって周波数分析したとき
の周波数精度を示す図、第６図は本発明のハードウェア
構成例を示す図、第７図はフィルタ設定用プロセッサの
処理を示す図、第８図は制御装置の従来例を示すブロッ
ク図であ。 １・・・コントローラ、２・・・力検出器、３・・・パ
ワー・アンプ、４・・・マニピュレータ、５・・・エン
コーダ、６・・・力１位置指令値、７・・・データ・メ
モリ・ボード、８・・・フィルタ設定用プロセッサ、１
ｏ・・・関節サーボ演算部、１１・・・位置算出部、１
２・・・力検出演算部、１３・・・力＆位置制御演算部
、１４・・・ノッチ・フィルタ演算部、１５・・・ＦＦ
Ｔ演算部、１６・・・定数算出部、１７・・・力覚セン
サ、１８・・・Ｎ設定部。 特許出廓人　　　　富士通株式会社

Claims (2)

【特許請求の範囲】
1. （１）マニピュレータ（４）と、 マニピュレータ（４）が受ける力を検出する力検出器（
   ２）と、 マニピュレータ（４）の現在位置を検出する位置検出部
   （２１）と、 これら検出された力及び位置並びに目標とする力及び位
   置に基づいて速度指令信号を発する力＆位置制御演算部
   （１３）と、 力＆位置制御演算部（１３）をから出力される速度指令
   信号の時系列を格納するメモリ部（７）と、上記速度指
   令信号に対して高速フーリエ変換演算を行う高速フーリ
   エ変換演算部（１５）と、高速フーリエ変換の結果得ら
   れる周波数スペクトルのピーク周波数を検出し、当該周
   波数がマニピュレータ（４）が追従可能な低域周波数を
   外れたものである場合に、その周波数成分を減衰させる
   ノッチ・フィルタの離散形演算式の係数を算出する定数
   算出部（１６）と、 力＆位置制御演算部（１３）の出力する速度指令信号の
   中から、上記係数で指定される周波数成分を減衰させる
   ノッチ・フィルタ演算部（１４）と、ノッチ・フィルタ
   演算部（１４）から出力される速度指令信号に基づいて
   マニピュレータ（４）を作動させる関節サーボ部（２０
   ）と を具備することを特徴とするマニピュレータ制御装置。
2. （２）マニピュレータ（４）と、 マニピュレータ（４）が受ける力を検出する力検出器（
   ２）と、 マニピュレータ（４）の現在位置を検出する位置検出部
   （２１）と、 これら検出された力及び位置並びに目標とする力及び位
   置に基づいて速度指令信号を発する力＆位置制御演算部
   （１３）と、 力＆位置制御演算部（１３）から出力される速度指令信
   号のＮ個の標本値系列を格納するメモリ部（７）と、メ
   モリ部（７）の標本値系列データに対して高速フーリエ
   変換演算を行う高速フーリエ変換演算部（１５）と、 設定したノッチ・フィルタ演算部（１４）によって所望
   の減衰が得られる周波数幅を周波数精度とした高速フー
   リエ変換演算が行なえるように、上記標本値系列の長さ
   Ｎを決定する長さ設定部（１８）と、高速フーリエ変換
   の結果得られる周波数スペクトルのピーク周波数を検出
   し、当該周波数がマニピュレータ（４）が追従可能な低
   域周波数を外れたものである場合に、その周波数成分を
   減衰させるノッチ・フィルタの離散形演算式の係数を算
   出する定数算出部（１６）と、 力＆位置制御演算部（１３）の出力する速度指令信号の
   中から、上記係数で指定される周波数成分を減衰させる
   ノッチ・フィルタ演算部（１４）と、ノッチ・フィルタ
   演算部（１４）から出力される速度指令信号に基づいて
   マニピュレータ（４）を作動させる関節サーボ部（２０
   ）と を具備することを特徴とするマニピュレータ制御装置。