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

Abstract

(57)【要約】 【目的】 マニピュレータの制御において、ツールの対
象物への接触時におけるくい込みやオーバーシュートを
防止する。 【構成】 接触判定手段Ｂは制御サイクルごとに力検出
手段Ａで検出された力またはモーメントの検出値Ｆs を
取り込み、予め指定された力しきい値Ｆthと比較を行
う。そして、検出値Ｆs が予め指定された制御サイクル
数ｍ以上連続して力しきい値Ｆth以上となった場合にツ
ールが対象物に接触したと判定する。変更サイクル数計
算手段Ｃは予め指定された仮想ばね定数変更時間Ｔw を
所定の制御サイクル時間ΔＴで分割し、仮想ばね定数を
変更する際の変更サイクル数Ｎを計算する。そして、仮
想ばね定数変更手段Ｄは、接触判定手段Ｂによりツール
が対象物に接触したと判定されると、マニピュレータＥ
の仮想ばね定数を予め指定された最終ばね定数ＫHendに
Ｎ回の制御サイクルで変更する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、マニピュレータ制御装
置に関し、特にツールが対象物に接触して作業を行う際
のマニピュレータの制御に好適なマニピュレータ制御装
置に関する。

【０００２】

【従来技術】マニピュレータによって、研磨作業や部品
のはめ込み作業など対象物に対して接触して作業を行う
場合、一般にツールを作業対象物まで移動させるときに
は位置制御を行い、ツールを作業対象物に接触させて作
業を行うときには力制御を行う。

【０００３】このような場合において作業を滑らかに行
うためには、位置制御と力制御の制御モードの切換えを
滑らかに行う必要がある。本出願人は、このような滑ら
かなモード切換えを実現する制御装置として、特開平２
−３１０６０９号に挙げられたマニピュレータの位置と
力の協調制御装置を提案した。

【０００４】図５は、この従来装置の原理構成図であ
る。

【０００５】図において、マニピュレータ部１は、マニ
ピュレータ各関節を駆動するためのマニピュレータ駆動
手段２と、アーム部３とより構成されている。アーム部
３の先端部には、マニピュレータが外部環境から受ける
力またはモーメント、あるいはその両者（以下、「力
等」と略す）をツール座標系で検索する力検出手段４が
設けられている。力検出手段４の出力は、力出力フィル
タリング演算手段４１を通過することにより、振動の原
因となる高周波数成分がカットされる。力出力フィルタ
リング演算手段４１の出力は手先力可変ゲイン演算手段
５に入力され、ここで位置と力の協調の程度に応じてゲ
インを変化させる演算処理が行われる。この手先力可変
ゲイン演算手段５では、例えば位置制御モードの成分に
対してはゲインを下げるような演算を行ない位置制御と
力制御の分離度を高め、より正確な位置制御及び力制御
ができるようにする。また、位置制御成分が力出力フィ
ードバック信号で不安定化するのを防止するようにす
る。

【０００６】位置と力の協調の程度はツール座標系での
各方向成分毎の仮想的なばね定数を増減することで行っ
ている。このばね定数の増減に応じて手先可変力ゲイン
演算手段５のゲインが増減される。位置制御モードの場
合には、仮想ばね定数が大の成分、すなわちより位置制
御を行いたい成分の力フィードバックゲインは小となる
ように減少される。逆に、力制御モードの場合には、力
制御を行いたい成分の仮想ばね定数を小とし、対応する
力制御成分の力フィードバックゲインは大となるように
増加される。力フィードバックゲインの増減は、例えば
仮想ばね定数とは逆比例の関係で増減するように設定す
る。

【０００７】出力トルク変換手段６は、手先力可変ゲイ
ン演算手段５からの出力に対し、例えば転置ヤコビ行列
を乗ずることにより、ツール座標系での力等をマニピュ
レータ各関節座標系での力等に変換する。

【０００８】力指令手段７では、力等をツール座標系で
表わした力指令信号を送出する。力指令信号には、力指
令フィルタリング演算手段７１により力出力フィルタリ
ング演算手段と同じ演算が施される。位置と力の協調の
程度に応じてゲインを変換させる演算処理を力指令フィ
ルタリング演算手段７１から送出された力指令信号に施
す指令力可変ゲイン演算手段８では、手先力可変ゲイン
演算手段５と同様の演算を行う。これは、指令値とフィ
ードバック信号のレベルを同じにするためである。指令
トルク変換手段９は、指令力可変ゲイン演算手段８から
の出力に、例えば転移ヤコビ行列を乗ずることでマニピ
ュレータ各関節座標系での力またはモーメントあるいは
その両者に変換する。

【０００９】マニピュレータの各関節の位置は位置検出
手段１０で検出される。また、マニピュレータの各関節
の指令位置は位置指令手段１１より送出される。

【００１０】位置検出手段１０から出力される位置信号
と位置指令手段１１から出力される指令位置との差とし
て求まる位置偏差を位置偏差検出手段１２で検出する。

【００１１】位置偏差トルク変換手段１３は、位置と力
の協調の程度に応じてツール座標系で設定された仮想的
なばね定数をマニピュレータの各関節座標系での仮想的
なばね定数に変換し、変換された仮想的なばね定数と位
置偏差検出手段１２から出力される位置偏差とに基づい
て位置偏差に応じた力等に変換する。

【００１２】速度検出手段１４は、マニピュレータ各関
節の速度を検出する。速度トルク変換手段１５は、ツー
ル座標系で設定された速度フィードバックゲインをマニ
ピュレータ各関節での速度フィードバックゲインに変換
し、変換された速度フィードバックゲインに基づいて、
速度検出手段１４より検出された速度をその速度に応じ
た力等に変換する。

【００１３】フィードバック補償演算手段１６では、指
令トルク変換手段９と位置偏差トルク変換手段１３の出
力の和が、出力トルク変換手段６と速度トルク変換手段
１５の出力の和と一致するようにフィードバック補償演
算する。

【００１４】フィードバック補償演算手段１６において
は、加減算演算手段１７によって、指令トルク変換手段
９より出力される力等の指令と位置偏差トルク変換手段
１３より出力される位置偏差に対応した力等の指令とを
加算し、出力トルク変換手段６より出力される力等の信
号と速度トルク変換手段１５より出力される速度に応じ
た力等の信号とを減算し、比例積分演算手段１８で加減
算演算手段１７より出力される偏差に比例、積分演算処
理や比例、積分、微分演算処理を施しマニピュレータ各
関節の駆動手段への力等の指令値とする。

【００１５】最終的にマニピュレータ各関節のアーム部
３は、フィードバック補償演算手段１６からの指令値に
基づいて動き、仮想ばね定数が小さい力制御モードの成
分においては、力指令値の少なくとも一方に一致するよ
うに力をツール端で発生しようとし、仮想ばね定数が大
きい位置制御モードの成分においては、位置指令値に一
致するように動作する。

【００１６】また、仮想ばね定数が中間的な値のとき
は、位置偏差に応じた力等がつり合う点まで動作しよう
とし、すなわちあたかもマニピュレータがばねで支えら
れているかのような動きを行う。

【００１７】このように、図５に示した従来技術によれ
ば、仮想ばね定数を連続的に変えることにより、位置制
御モードと力制御モードを滑らかに切換えることができ
た。しかしながら、この従来技術には、ツールが対象物
に接触する場合やツールがその対象物上を移動しながら
作業を行う場合の位置制御モードと力制御モードの具体
的な切換え方法が示されていなかった。

【００１８】そこで、具体的な制御モードの切換えの方
法としては、例えば、予めプログラムや教示によりマニ
ピュレータに対して仮想ばね定数の変更パターンを指示
しておく、という方法が考えられるが、この方法を用い
た場合、実際の作業時において対象物の位置ずれがある
とさまざまな不具合を生じるという問題があった。すな
わち、対象物がツールに近い方にずれた場合は、位置制
御に重みがかかった状態で対象物に接触するため、対象
物に対するツールのくい込みが生じ、反対に対象物がツ
ールから遠い方にずれた場合は、力制御に重みがかかっ
た状態で対象物に接触するため、接触時に力のオーバー
シュートが生じる可能性があった。

【００１９】この問題に対処する方法としては、ツール
と対象物との接触判定を行い、接触後ただちに位置制御
モードから力制御モードに切り換えるという方法が考え
られる。このようなモード切換えの技術が特開平３−１
８４７８９号において提案されている。

【００２０】この技術は、位置制御モード、力制御モー
ドの各場合で速度指令値を計算し、各速度指令値を逆ヤ
コビ行列を用いて各関節での速度指令値に変換し、モー
タをサーボ制御する。

【００２１】位置制御モードから力制御モードへの切換
えは、マニュピュレータが対象物に接触し、力検出器の
力検出値Ｆs が予め設定した値からの力しきい値Ｆthを
越えたとき、位置制御モードから力制御モードへの移行
を開始する。その後、力検出値Ｆs が目標力Ｆｏに達し
た後は完全な力制御モードとなる。

【００２２】このように、このモード切換えの従来技術
によれば、ツールの接触と連動してモード切換えができ
るので、前述したような対象物の位置ずれによる諸問題
が生じない。

【００２３】

【発明が解決しようとする課題】しかしながら、このモ
ード切換えに関する従来技術では、力検出値Ｆs と予め
設定されたスレッショルド値Ｆthを比較し、位置制御と
力制御を切り換えるが、しきい値Ｆth付近で力検出値Ｆ
s にノイズが乗った場合には、Ｆs がＦth付近で振動
し、Ｆthを越えたり越えなかったりを繰り返すことがあ
り、このため位置制御と力制御が頻繁に切り換わること
になり、マニピュレータの状態が著しく不安定となると
いう問題があった。

【００２４】また、制御モード切り換え前後で、位置制
御モード、力制御モードにおける速度指令値が一致する
とは限らないので、速度の突変が起こり、サーボ系に著
しいショックが起きる場合がある。これを防ぐには速度
指令値を一致させるように加減速を加える必要がある
が、加減速生成を行う複雑な計算が必要となるとう問題
があった。

【００２５】本発明は、上記従来の問題点を解決するた
めになされたものであり、対象物に対するツールの接触
及び離脱時における力のオーバーシュートやツールのく
い込みを防ぎ、安定した動作で滑らかに制御モードを切
り換えることのできるマニピュレータ制御装置を提供す
ることを目的とする。

【００２６】

【課題を解決するための手段】上記問題を解決するため
に、本発明の第１の構成では、各制御サイクルごとにツ
ールが外部環境から受ける力及びモーメントのうち少な
くとも一方を検出する力検出手段と、各制御サイクルご
とに前記力検出手段の検出値を力しきい値と比較し、前
記検出値が予め指定された制御サイクル数以上連続して
前記力しきい値以上となった場合に前記ツールが対象物
に接触した判定する接触判定手段と、予め指定された仮
想ばね定数変更時間と所定の制御サイクル時間とに基づ
いて、マニピュレータの仮想ばね定数を変更するときの
変更サイクル数を算出する変更サイクル数計算手段と、
前記接触判定手段によって接触と判定された時点から、
マニピュレータの仮想ばね定数を予め指定された最終仮
想ばね定数に前記変更サイクル数で変更する仮想ばね定
数変更手段とを含むことを特徴とする。

【００２７】また第２の構成では、前記第１の構成にお
いて、ツールと前記対象物との距離を検出する距離検出
手段と、検出された距離と予め指定された距離しきい値
とを比較し、前記距離が前記距離しきい値以下になった
ときに接近状態と判定する接近判定手段と、前記接近判
定手段及び接触判定手段のうち少なくとも一方の判定結
果に基づいてマニピュレータの速度及び加速度を変更す
る速度・加速度変更手段とを含み、前記速度・加速度変
更手段は、前記接近判定手段により接近状態と判定され
るとマニピュレータの速度を所定の接近速度に変更し、
前記接触判定手段により接触と判定されるとマニピュレ
ータの速度を所定の作業速度に変更することを特徴とす
るさらに第３の構成では、前記第２の構成において、仮
想ばね定数変更手段が、前記接近判定手段により接近状
態と判定された時点から前記接触判定手段により接触と
判定されるまでの間に、マニピュレータの仮想ばね定数
を予め指定された接触時仮想ばね定数に変更することを
特徴とする。

【００２８】また第４の構成では、マニピュレータの軌
跡を表す一連の位置指令値及びその位置指令値に対応し
た速度設定値を出力するとともに、前記一連の位置指令
値のうち所定の位置指令値から前記位置指令値に対応し
た速度設定値として所定の接近速度を出力する位置指令
手段と、各制御サイクルごとに前記ツールが外部環境か
ら受ける力及びモーメントのうち少なくとも一方を検出
する力検出手段と、各制御サイクルごとに前記力検出手
段の検出値を予め指定された力しきい値と比較し、前記
検出値が予め指定された制御サイクル数以上連続して前
記力しきい値以上となったときに前記ツールが対象物に
接触した判定する接触判定手段と、前記接触判定手段の
判定結果に基づいてマニピュレータの速度及び加速度を
変更する速度・加速度変更手段とを含み、前記速度・加
速度変更手段は、前記接触判定手段でツールが対象物に
接触したと判定されるとマニピュレータの速度を予め指
定された作業速度に変更することを特徴とする。

【００２９】さらに第５の構成では、前記第４の構成に
おいて、各制御サイクルごとに前記力検出手段の検出値
を予め指定された第２の力しきい値と比較し、前記検出
値が予め指定された制御サイクル数以上連続して前記第
２の力しきい値以下となったときに、前記ツールが前記
対象物から離脱したと判定する離脱判定手段を含み、前
記速度・加速度変更手段は、さらに、前記離脱判定手段
でツールが対象物から離脱したと判定されると、マニピ
ュレータの速度を所定の離脱速度まで上昇させることを
特徴とする。

【００３０】

【発明の作用及び効果】本発明の第１の構成では、図１
に示すように、接触判定手段Ｂは制御サイクルごとに力
検出手段Ａで検出された力またはモーメントの検出値Ｆ
s を取り込み、予め指定された力しきい値Ｆthと比較を
行う。そして、検出値Ｆs が予め指定された制御サイク
ル数ｍ以上連続して力しきい値Ｆth以上となった場合に
ツールが対象物に接触したと判定する。変更サイクル数
計算手段Ｃは予め指定された仮想ばね定数変更時間Ｔw
を所定の制御サイクル時間ΔＴで分割し、仮想ばね定数
を変更する際の変更サイクル数Ｎを計算する。そして、
仮想ばね定数変更手段Ｄは、接触判定手段Ｂによりツー
ルが対象物に接触したと判定されると、マニピュレータ
Ｅの仮想ばね定数を予め指定された最終ばね定数ＫHend
にＮ回の制御サイクルで変更する。

【００３１】このように、第１の構成によれば、接触判
定手段Ｂは、検出値Ｆs が予め指定された制御サイクル
数以上連続して力しきい値Ｆth以上となったときに初め
て接触したと判定するので、力検出手段の検出信号のノ
イズによる接触の誤判定を防ぐことができ、制御モード
の円滑な切換えが可能となる。また、位置制御モードか
ら力制御モードへの制御モードの切換えを仮想ばね定数
の切換えによって行っているので、マニピュレータの速
度の突変が起こらず、滑らかに制御モードを切換えるこ
とができる。

【００３２】また第２の構成では、図１に示すように、
接近判定手段Ｆは距離検出手段で検出された距離Ｓl を
予め指定された距離しきい値ｌと比較して、距離Ｓl が
距離しきい値ｌ以下になったときに接近状態と判定す
る。接近判定手段Ｆにより接近状態と判定されると、速
度・加速度変更手段Ｇはマニピュレータの速度及び加速
度をそれまでの値から所定の接近速度に変更し、ツール
が対象物に対して緩やかに接触できるようにする。そし
て、接触判定手段Ｂで接触と判定されると、速度・加速
度変更手段Ｇはマニピュレータの速度及び加速度を所定
の作業速度まで増加させる。このような構成により、接
近状態と判定されるとマニピュレータが低速となり、ツ
ールを対象物に対して緩やかに接触させることができる
ので、ツールのくい込みや力のオーバーシュートなどを
防ぐことができる。逆に言えば、プログラムあるいは教
示によるマニピュレータの運行指令を、対象物に向かっ
て移動するときに高速で移動するように最初に設定して
おいたとしても、この構成によれば、ツールが対象物に
対してある距離まで近付くと自動的に接触に適切な速度
まで減速することができる。

【００３３】さらに第３の構成では、仮想ばね定数変更
手段Ｄは、前記接近判定手段により接近状態と判定され
た時間より仮想ばね定数の変更を開始し、ツールが対象
物に接触するまでの間に仮想ばね定数を予め指定された
接触時仮想ばね定数に変更する。このようにツールが対
象物に接触する前に、マニピュレータ仮想ばね定数を予
めある程度まで最終仮想ばね定数に近付けておくことに
より、接触した後の仮想ばね定数の変更を短時間で円滑
に行うことができる。

【００３４】また第４の構成では、まず位置指令手段
は、マニピュレータの軌跡を表す一連の位置指令値及び
その位置指令値に対応した速度設定値を出力する。この
とき、前記一連の位置指令値のうち所定の位置指令値以
降は前記位置指令値に対応した速度設定値として所定の
接近速度を出力する。これにより、マニピュレータは、
ツールが対象物に向かって移動する途中の所定の位置か
ら、所定の接近速度で移動するようになる。

【００３５】そして、接触判定手段Ｂで接触と判定され
ると、速度・加速度変更手段はマニピュレータの速度を
予め指定された作業速度に変更する。

【００３６】これにより、ツールを対象物に対して緩や
かな接近速度で接触させることができるので、ツールの
くい込みや力のオーバーシュートなどを防ぐことができ
る。さらに第５の構成では、離脱判定手段は、力検出手
段の検出値と第２の力しきい値とを比較し、検出値が予
め指定された制御サイクル数以上連続して第２の力しき
い値以下となると、ツールが対象物から離脱したと判定
する。そして、離脱判定手段によりツールが対象物から
離脱したと判定されると、速度・加速度変更手段はマニ
ピュレータの速度を所定の離脱速度まで上昇させる。こ
れにより、対象物からの離脱時にマニピュレータを高速
で運行することができるので、作業サイクルが短縮でき
る。

【００３７】さて、このような本発明の構成を図５に示
した従来構成に付加したのが図２のブロック図に示す構
成である。以下、図２を参照して、さらに詳細に本発明
に係るマニピュレータ制御装置の動作を説明する。な
お、図５と同様の構成要素には同様の符号を付して、そ
の説明を省略する。

【００３８】（１）まず、本発明の第１の構成を用いた
場合について説明する。

【００３９】マニピュレータによる作業を行う場合、ま
ず操作者が制御パラメータ設定手段３１にて後に詳述す
る様々な制御パラメータを設定する。このようなパラメ
ータとしては、例えば、実験によって求められた値や操
作者の経験から得られた値などが用いられる。この制御
パラメータを用いて、具体的には以下のようにマニピュ
レータの制御が行われる。

【００４０】マニピュレータのアーム部３の先端に取り
付けられた力検出手段４から得られた力及びモーメント
のうち少なくとも一方の検出値Ｆs は接触判定手段３２
に入力される。接触判定手段３２では、各制御サイクル
ごとに、検出値Ｆs と制御パラメータ設定手段３１で予
め設定された力しきい値Ｆthとを比較する。そして、接
触判定手段３２は、検出値Ｆs がｍサイクル以上連続し
て力しきい値Ｆth以上となると、ツールが対象物に接触
したと判定し、接触検出信号を仮想ばね定数変更手段３
４に送信する。なお、この判定のためのサイクル数ｍは
制御パラメータ設定手段３１で予め設定される。

【００４１】変更サイクル数計算手段３３は、制御パラ
メータ設定手段３１で予め設定された仮想ばね定数変更
時間Ｔw をマニピュレータの制御に用いるコンピュータ
等の能力によって決まる制御サイクル時間ΔＴで分割し
て、仮想ばね定数を変更する際の変更サイクル数Ｎを求
める。

【００４２】仮想ばね定数変更手段３４は、接触判定手
段３２から接触検出信号を受けとると、仮想ばね定数の
変更を開始する。このとき、仮想ばね定数変更手段３４
は、制御パラメータ設定手段３１で予め設定された最終
仮想ばね定数ＫHendと、仮想ばね定数現在値バッファ３
９に格納されている仮想ばね定数の現在値ＫH(cur)と、
変更サイクル数計算手段３３によって得られた変更サイ
クル数Ｎとを用いて、１制御サイクル当たりの仮想ばね
定数の変更量を計算する。そして、各制御サイクルごと
にその制御サイクルにおける仮想ばね定数の初期値ＫH
(n)にその変更量を加えることにより次の制御サイクル
における仮想ばね定数ＫH(n+1)を求める。求められた次
サイクル時の仮想ばね定数ＫH(n+1)は、手先力可変ゲイ
ン演算手段５、指令力可変ゲイン演算手段８および位置
偏差トルク変換手段１３に対して出力される。そして、
手先力可変ゲイン演算手段５、指令力可変ゲイン演算手
段８および位置偏差トルク変換手段１３は、仮想ばね定
数変更手段３４からの次サイクル時の仮想ばね定数ＫH
(n+1)に基づいてフィードバックのゲインを調整するこ
とにより、マニピュレータの仮想ばね定数を変更する。

【００４３】このようにして、マニピュレータの仮想ば
ね定数が、接触判定時よりＮサイクルかけて徐々に最終
仮想ばね定数ＫHendに変更される。

【００４４】これらの構成によれば、接触の判定を高精
度で行うことができるため、制御モードの切換えを安定
して行うことができる。また、仮想ばね定数を徐々に変
えるので、制御モードの切換えを滑らかに行うことがで
きる。

【００４５】（２）次に、本発明の第２及び第３の構成
を用いた場合について説明する。

【００４６】第２および第３の構成は、前述の第１の構
成に接近判定手段３８と速度・加速度変更手段３６を加
えたものである。

【００４７】ツールと対象物との距離Ｓl はアーム部３
に取り付けられた距離センサによって検出される。検出
された距離Ｓl は接近判定手段３８に入力される。接近
判定手段３８では、制御パラメータ設定手段３１で予め
設定された距離しきい値ｌと距離Ｓl とを比較し、距離
Ｓl が距離しきい値ｌ以下となったときにツールが対象
物に接近したと判定して、接近検出信号を出力する。

【００４８】第２の構成では、この接近検出信号は速度
・加速度変更手段３６に入力される。接近検出信号を受
信すると、速度・加速度変更手段３６はマニピュレータ
の速度・加速度設定値を所定の接近速度まで低下させ、
これによりマニピュレータを接触時にショックを生じな
い程度の速度まで減速させる。すなわち、速度・加速度
変更手段３６は、接近検出信号を受信すると、プログラ
ムや教示等で与えられていた速度・加速度設定値を制御
パラメータ設定手段３１で予め設定された減速パラメー
タｘ１で除して、新たな速度・加速度設定値として速度
設定部３７に渡す。位置指令手段１１では速度設定部３
７で新たに設定された速度・加速度設定値を位置指令値
に変換して、マニピュレータの制御に用いる。このとき
の、減速パラメータｘ１は、プログラムや教示によって
指定された速度とマニピュレータの特性によって決まる
接近速度とを考慮してユーザが予め設定する。

【００４９】このようにして、接近判定手段３８で接近
状態と判定されると、マニピュレータは減速され、接触
するまで低速状態で移動する。その後、接触判定手段３
２によってツールが対象物に接触したと判定されると、
接触検出信号が速度・加速度変更手段３６に送信され
る。接触検出信号を受けとった速度・加速度変更手段３
６は、マニピュレータの接近時の速度・加速度設定値に
予め設定された増速パラメータｘを乗じて速度設定部３
７に渡し、マニピュレータの速度を作業時に必要な作業
速度にまで増速する。このようにして、マニピュレータ
を接触時には低速で緩やかに接触させ、作業時には作業
に必要な速度で移動させることができる。したがって、
接触時のツールのくい込みや力のオーバーシュートを防
ぐことができる。

【００５０】また、第３の構成では、接近判定手段３８
から発せられた接近検出信号は、速度・加速度変更手段
３６だけでなく、仮想ばね定数変更手段３４にも送信さ
れる。仮想ばね定数変更手段３４は、接近検出信号を受
信すると、仮想ばね定数を予め制御パラメータ設定手段
３１で設定された接触時仮想ばね定数ＫHiに変更する。
接触時仮想ばね定数ＫHiとしては、最終仮想ばね定数Ｋ
HENDに至るまでの中間的な値を、接触時に適切な値とし
て操作者が予め設定しておく。この接近時における仮想
ばね定数の変更は、前述した第１の構成の場合と同様に
徐々に行われるが、この際の仮想ばね定数変更時間は距
離しきい値ｌと適合するように操作者が設定する。この
仮想ばね定数変更時間を制御サイクル時間ΔＴで分割し
て変更サイクル数を求め、接触時仮想ばね定数ＫHiを目
標値として前述した第１の構成の場合と同様に徐々に仮
想ばね定数の変更を行う。

【００５１】そして、ツールが対象物に接触した後の仮
想ばね定数の変更は、接触時仮想ばね定数ＫHiを開始点
として、前述した第１の構成の場合と同様に行われる。

【００５２】このように、第３の構成によれば、マニピ
ュレータの仮想ばね定数を、ツールが対象物に接触する
前に、マニピュレータの仮想ばね定数をある程度まで最
終仮想ばね定数ＫHENDに近付けておくことにより、接触
判定後の仮想ばね定数の変更量を少なくすることができ
るので、仮想ばね定数の変更がさらに滑らかに行える。

【００５３】（３）次に、本発明の第４及び第５の構成
を用いた場合について説明する。

【００５４】位置指令手段１１は、プログラムや教示等
に基づいてマニピュレータの軌跡を表す一連の位置指令
値及びそれら位置指令値に対応した速度設定値をマニピ
ュレータに対して指令する。位置指令手段１１はこのと
き、一連の位置指令値のうち、マニピュレータが対象物
に向かって移動する途中の、ある所定の位置からの速度
設定値を所定の接近速度に設定する。この接近速度は、
ツールが対象物に接触したときに力のオーバーシュート
やツールのくい込みが起こらない程度の速度であり、個
々のマニピュレータの特性によって異なる。従って、マ
ニピュレータは、対象物に向かって移動する時、途中の
ある所定の位置にくると接近速度まで減速されることに
なる。そして、接触判定手段３２で接触と判定されるま
では、マニピュレータは、その接近速度で移動する。

【００５５】マニピュレータのアーム部３の先端に取り
付けられた力検出手段４から得られた検出値Ｆs は接触
判定手段３２に入力される。接触判定手段３２では、各
制御サイクルごとに、検出値Ｆs と制御パラメータ設定
手段３１で予め設定された力しきい値Ｆthとを比較す
る。そして、接触判定手段３２は、検出値Ｆs がｍサイ
クル以上連続して力しきい値Ｆth以上となると、ツール
が対象物に接触したと判定し、接触検出信号を速度・加
速度変更手段３６に送信する。

【００５６】速度・加速度変更手段３６は、接触検出信
号を受信すると、接近時の速度及び加速度に予め制御パ
ラメータ設定手段３１で設定された増速パラメータｘを
乗じて、新たな速度・加速度設定値として速度設定部３
７に渡す。このようにして、マニピュレータの速度が、
接近速度から予め設定された作業速度まで増速される。
これにより、ツールを対象物に対して緩やかに接触させ
ることができるので、ツールのくい込みや力のオーバー
シュートなどを防ぐことができる。

【００５７】第５の構成では、さらに離脱判定手段が設
けられている。この離脱判定手段は、前述の接触判定手
段と逆の動作を行う。すなわち、離脱判定手段は、力検
出手段４で検出された検出値Ｆs を離脱判定用の第２の
力しきい値と比較し、検出値Ｆs がｍ´サイクル以上連
続して前記第２の力しきい値以下となったときに、ツー
ルが対象物から離脱したと判定し、離脱検出信号を速度
・加速度変更手段３６に送信する。この判定のためのサ
イクル数ｍ´は接触判定の場合のｍと同じ値でもよい。

【００５８】離脱検出信号を受信した速度・加速度変更
手段３６は、作業時のマニピュレータの速度・加速度に
対して増速パラメータを乗じて新たな速度・加速度設定
値とし、その値を速度設定部３７に渡す。この際の増速
パラメータは、制御パラメータ設定手段３１で予め設定
される。この構成によれば、接触時の作業が終了する
と、ツールが対象物から高速な離脱速度で離脱すること
ができるため、全体的な作業サイクルを短縮できる。

【００５９】

【実施例】本発明の実施例を図面に基づいて説明する。
図３は、本発明に係るマニピュレータ制御装置を示すブ
ロック図である。図３に示した装置は、図２に示した構
成を具体化したものである。したがって、図３において
は、図２の部材に対応する部材には図２と同じ符号を付
している。

【００６０】［第１実施例］図３を参照して本発明の第
１実施例について説明する。以下の説明では、まず、最
初にマニピュレータ制御装置全体のフィードバック制御
の流れを説明し、その後に具体的な仮想ばね定数の変更
機構について説明する。

【００６１】フィードバック制御の流れ 図３において６軸マニピュレータ１Ａの手首部にはツー
ルが外部環境から受ける力またはモーメントあるいはそ
の両者（以下、力等と略す）を測定するための６軸力セ
ンサ４Ａが装着されている。６軸力センサ４Ａの検出値
Ｆs は安定性を増すために、必要な周波数帯域を越える
信号をカットする力出力フィルタリング演算手段４１に
よりフィルタリングされる。例えば、力出力フィルタリ
ング演算手段４１をローパスフィルタをして最も簡単な
一時遅れフィルタをした場合、入力ｈ（ｓ）と出力ｆ
（ｓ）が、 ｆ（ｓ）＝１／（１＋Ｔc ・ｓ）・ｈ（ｓ） の関係を持つ演算を行う。ここでｓはラプラス変換を示
し、Ｔc は時定数を示す。時定数Ｔc はカットしたい周
波数に応じて各成分ごとに任意に設定される。

【００６２】力出力フィルタリング演算手段４１でフィ
ルタリングされたツール座標系での検出値Ｆs には、次
に、力ループと位置ループの分離度をよくするために、
手先力可変ゲイン演算手段５によりフィードバックゲイ
ンＫffb が乗ぜられる。フィードバックゲインＫffb は
例えば、仮想ばね定数ＫH と分離パラメータＦBFCTを用
いて、 Ｋffb ＝ＦBFCT／（ＫH ＋ＦBFCT） として設定することができる。ここで、仮想ばね定数Ｋ
H は、力制御に重みがあるときは小さく、位置制御に重
みがあるときは大きくなるので、ＫH ＝０の完全な力制
御モードのときにはＫffb ＝１となり１００％力フィー
ドバックがかかる。そして、位置制御モードとなるに従
ってＫH が大きくなるので、Ｋffb は小さくなり（＜
１）、力フィードバックがかからなくなる。分離パラメ
ータＦBFCTの大きさは仮想ばね定数ＫH とのかねあいで
必要に応じユーザが各成分ごとに任意に設定できる。

【００６３】ここで、仮想ばね定数ＫH の大きさを連続
的に変えれば、制御モード切換え時にショックを生ずる
ことなく滑らかに切換えることができる。

【００６４】手先力可変ゲイン演算手段５からの出力は
次に、マニピュレータの現在の位置、姿勢より決まるヤ
コビ行列Ｊの転置行列Ｊ^T を乗ずる出力トルク変換手段
６によりマニピュレータ各関節座標系での力等の値に変
換される。

【００６５】力指令手段７では、力等をツール座標系で
表わした力指令信号を送出する。

【００６６】力指令手段７からの力指令信号には力指令
フィルタリング演算手段７１により力出力フィルタリン
グ演算手段４１と同じ演算を施し低周波数成分のみを通
過させる。これは、力フィードバックの遅れと同様に指
令値も遅らせるためである。指令力可変ゲイン演算手段
８では、力指令フィルタリング演算手段７１から送出さ
れる力指令信号に手先力可変ゲイン演算手段５と同様に
仮想ばね定数の大きさに応じた力フィードバックゲイン
を乗じ、ゲインを変える。これは、フィードバックの大
きさと指令値の大きさを同じにするためである。

【００６７】指令トルク変換手段９では、転置ヤコビ行
列Ｊ^T を用いて出力トルク変換手段６と同じ演算を行
い、指令力可変ゲイン演算手段８からの出力をマニピュ
レータ各関節座標系での指令値に変換する。

【００６８】マニピュレータの各軸の位置の検出はモー
タ軸に取り付けられたエンコーダなどの位置検出手段１
０によりおこなわれる。

【００６９】マニピュレータ各軸の指令位置は位置指令
手段１１により送出される。

【００７０】位置検出手段１０から出力される位置信号
と位置指令手段１１から出力される指令位置との差とし
て求まる位置偏差は、位置偏差検出手段１２により算出
される。

【００７１】本実施例では、仮想ばね定数を調節するこ
とで、力制御と位置制御の調整を行う。すなわち、仮想
ばね定数ＫH が大きいとき（したがって、コンプライア
ンスは小さい）は位置制御となり、仮想ばね定数ＫH が
小さいとき（コンプライアンスは大きい）は力制御とな
る。仮想的なばね定数ＫH はツール座標系で設定され
る。位置偏差トルク変換手段１３は、ツール座標系で設
定された仮想ばね定数ＫH をマニピュレータの各軸座標
系での仮想的なばね定数Ｋθに変換し、位置偏差検出手
段１２から出力される位置偏差信号にばね定数Ｋθを乗
じ、位置偏差に応じた力またはモーメントに変換する。

【００７２】速度検出手段１４は、位置検出手段１０か
ら得られる位置信号を微分演算することによりマニピュ
レータ各軸の速度を求める。

【００７３】速度トルク変換手段１５は、ツール座標系
で設定された速度フィードバックゲインＫvHをマニピュ
レータ各関節での速度フィードバックゲインに変換し、
これと速度検出手段１４より出力される速度信号とを乗
じて、マニピュレータの速度に応じた力等に変換する。

【００７４】フィードバック補償演算手段１６は、指令
トルク変換手段９より出力される力指令値と位置偏差ト
ルク変換手段１３より出力される位置偏差に対応した力
指令値とを加算し、その加算値から出力トルク変換手段
６より出力される力検出値と速度トルク変換手段１５よ
り出力される速度に応じた力検出値とを減算し、その偏
差に比例、積分などの演算処理を施しマニピュレータ各
軸の駆動手段への力等の指令値とする。

【００７５】この制御系の演算式を示すと以下のように
なる。なお、以下では力とモーメントの両者を使用する
場合について説明する。

【００７６】今、マニピュレータを指令値に従って駆動
するのに必要な力とモーメントをＦc （ツール座標系）
とすると、Ｆc は、力指令可変ゲイン演算手段８より出
力される力とモーメント指令Ｆi （ツール座標系）と、
目標位置Ｐi （ワールド座標系）とツールの現在位置Ｐ
o （ワールド座標系）との偏差Ｐi −Ｐo （ツール座標
系）に仮想ばね定数ＫH （ツール座標系）をかけたもの
と、ツールの速度Ｖo（ツール座標系）に速度フィード
バックゲインＫvH（ツール座標系）をかけたものとの和
となり、 Ｆc ＝Ｆi ＋ＫH （Ｐi −Ｐo ）＋ＫvH・Ｖo ・・・(1) となる。ここでは慣性項は無視した。Ｆc をマニピュレ
ータ各軸のトルクτc に変換する。ここでマニピュレー
タのヤコビ行列をＪとおくと、 τc ＝Ｊ^T Ｆc ＝Ｊ^T Ｆi ＋Ｊ^T ＫH （Ｐi −Ｐo ）＋Ｊ^T ＫvH・Ｖo ・・・(2) ここで、^T は転置行列を示す。また、マニピュレータ各
軸目標値θi 、マニピュレータ各軸現在値θo を用い
て、 Ｐi −Ｐo ＝ΔＰ＝ＪΔθ＝Ｊ（θi −θo ） ・・・(3) Ｖo ＝Ｊ・ωo ・・・(4) と近似できる。ここで、ωo は、マニピュレータ各軸現
在値θo の時間微分値である。式(3) 及び(4) を(2) へ
代入すると τc ＝Ｊ^T Ｆc ＝Ｊ^T Ｆi ＋Ｊ^T ＫH Ｊ・（θi −θo ）＋Ｊ^T ＫvHＪ・ωo ・・(5) となる。ここで、 τi ＝Ｊ^T Ｆi Ｋθ＝Ｊ^T ＫH Ｊ Ｋv θ＝Ｊ^T ＫvHＪ とおくと、 τc ＝τi ＋Ｋθ（θi −θo ）＋Ｋv θ・ωo ・・・ (6) となり、マニピュレータ各軸レベルの角度、各速度を用
いることができる。

【００７７】ただし、ここで、 τi ：トルク指令値（マニピュレータ各軸レベル） Ｋθ ：仮想ばね定数（マニピュレータ各軸レベル） Ｋv θ：速度フィードバックゲイン（マニピュレータ各
軸レベル） である。

【００７８】位置偏差トルク変換手段１３では式(6) の
Ｋθ（θi −θo ）の部分の演算を行い、速度トルク変
換手段１５では式(6) のＫv θ・ωo の部分の演算を行
っている。

【００７９】指令トルク変換手段９では指令力可変ゲイ
ン演算手段８の出力をＦi として τi ＝Ｊ^T Ｆi の演算を行っている。

【００８０】出力トルク変換手段６では手先力可変ゲイ
ン演算手段５の出力をＦo として τo ＝Ｊ^T Ｆo の演算を行っている。

【００８１】マニピュレータ各軸のサーボ系はトルクル
ープとなっており、フィードバック補償演算手段１６
は、指令トルク変換、位置偏差トルク変換、速度トルク
変換の各手段の出力を用い、マニピュレータ各軸のトル
ク指令値τc を式(6) に従って、加減算演算を行って求
め、更に出力トルク変換手段６より得られるマニピュレ
ータ各関節の出力トルクτo がτc に一致するようにフ
ィードバック制御演算を行っている。フィードバック制
御演算では、比例ゲインＫp 及び積分ゲインＫIにより
比例・積分制御を行うことができる。更に必要に応じて
位相補償などを加えることも可能である。また、マニピ
ュレータの位置、姿勢に応じた重力トルク補償Ｃｇも加
えることもできる。

【００８２】フィードバック補償演算によって得られた
演算結果は、マニピュレータの駆動手段への力およびモ
ーメントの少なくとも一方の指令値となる。この指令値
は、駆動手段としてモータドライバ１７１とモータとを
用いる場合はモータの電流指令値としてモータ電流値に
換算して与えられる。モータは減速機を通してマニピュ
レータ各関節のアーム部を駆動し、最終的にマニピュレ
ータが所望の動きを行うことが可能となる。マニピュレ
ータが動いた結果の信号は位置検出手段および力検出手
段によって検出され、フィードバック信号として必要な
次の演算手段に入力され、繰り返し演算が行われる。

【００８３】このように、仮想ばね定数ＫH は、手先力
可変ゲイン演算手段５及び指令力可変ゲイン演算手段８
において、フィードバックゲインＫffb の制御に用いら
れている（式(2) 参照）。また、仮想ばね定数ＫH は、
位置偏差トルク変換手段１３において位置偏差をその位
置偏差に応じたトルク指令値に変換するときのばね定数
として用いられている。この仮想ばね定数ＫH を徐々に
変更していくことによりマニピュレータの剛性を変え、
位置制御モードと力制御モードの重みを任意に変えるこ
とができる。

【００８４】仮想ばね定数の変更 上述したようなフィードバック制御系において、本実施
例では以下に示すようにして、仮想ばね定数を変更し、
制御モードの切換えを行う。この仮想ばね定数の変更過
程が図４のフローチャートに示されている。

【００８５】まず、６軸力センサ４Ａは、制御サイクル
ごとにツールが受ける力またはモーメントあるいはその
両者を検出し、検出値Ｆs を出力する。接触判定手段３
２は、６軸力センサ４Ａより検出値Ｆs を受けとり（Ｓ
１０）、検出値Ｆs と制御パラメータ設定手段３１で予
め設定された力しきい値Ｆthとを以下に示す式(a) を用
いて制御サイクルごとに比較する（Ｓ１２）。

【００８６】 Ｆs ≧Ｆth ・・・(7) そして、接触判定手段３２は、式(7) が成立するとカウ
ント値ＣＮＴを１つインクリメントし（Ｓ１４）、成立
しないときはカウント値ＣＮＴをクリアする（Ｓ２
６）。従って、カウント値ＣＮＴは式(7) が連続して成
立しない場合には０にクリアされてしまう。

【００８７】さらに、接触判定手段３２では、カウント
値ＣＮＴを制御パラメータ設定手段３１で予め設定され
た接触判定基準回数ｍと、式(8) を用いて比較する（Ｓ
１６）。

【００８８】 ＣＮＴ≧ｍ ・・・(8) そして、式(8) が成立すると、接触判定手段３２は、ツ
ールが対象物に接触したと判定し、接触検出信号を発す
る。逆に、式(8) が成立しない場合は、位置制御モード
を継続すると判定し、接触検出信号は発しない。

【００８９】なお、接触判定手段３２で用いられるパラ
メータＦth及びｍは、ユーザが作業の前に制御パラメー
タ設定手段３１によって設定するのであるが、この設定
の際には、力しきい値Ｆthについては対象面の硬さや接
触時の力のオーバーシュート等を考慮して設定する必要
があり、また、触判定基準回数ｍについては６軸力セン
サ４Ａのノイズの程度や力しきい値Ｆthの大きさを考慮
して設定する必要がある。例えば、接触判定基準回数ｍ
の設定においては、ｍが大きすぎると位置制御モードで
の接触時間が長くなるためツールのくい込みが起こりや
すくなり、逆にｍが小さすぎるとセンサのノイズによる
誤判定が起こりやすくなる。したがって、設定の際に
は、それらのかねあいを考慮してｍの値を決定する必要
がある。そのような設定値としては、例えば、実験等に
より求めた値や、ユーザの経験的知識から得られた値な
どを用いる。

【００９０】式(8) が成立した場合、接触判定手段３２
から出力された接触検出信号は、仮想ばね定数変更手段
３４に入力される。仮想ばね定数変更手段３４では、接
触検出信号を受けると、仮想ばね定数の変更を開始す
る。

【００９１】ここで、変更サイクル数計算手段３３は、
制御パラメータ設定手段３１で予め設定された仮想ばね
定数変更時間Ｔw とコンピュータの能力によって決まる
所定の制御サイクル時間ΔＴとから、式(9) にしたがっ
て仮想ばね定数の変更を行う際の変更サイクル数Ｎを求
める（Ｓ１８）。

【００９２】 Ｎ＝Ｔw/ΔＴ ・・・(9) なお、このＴw は、仮想ばね定数の変更の際にマニピュ
レータの動作にショック等が生じないように、適切な長
さにする。Ｔw の値は、例えば、実験やユーザの経験的
知識に基づいて設定する。

【００９３】仮想ばね定数の変更開始時において、仮想
ばね定数変更手段３４は、仮想ばね定数現在値バッファ
３９から仮想ばね定数の現在値ＫH(cur)を読み出すとと
もに、その現在値ＫH(cur)と、予め制御パラメータ設定
手段３１で設定された最終仮想ばね定数ＫHENDと、変更
サイクル数計算手段３３で求められた変更サイクル数Ｎ
とにより、次式(10)を用いて１制御サイクル当たりの仮
想ばね定数変更量ΔＫH を求める（Ｓ２０）。

【００９４】 ΔＫH ＝（ＫHEND−ＫH(cur)）／Ｎ ・・・(10) ここで、仮想ばね定数現在値バッファ３９は位置偏差ト
ルク変換手段１３から仮想ばね定数の現在値を検出して
いる。

【００９５】なお、最終仮想ばね定数ＫHENDは、例えば
グラインダなどのツールが対象物に接触して作業を行う
場合に最適な仮想ばね定数の値であり、この値は、ユー
ザが作業開始時に実験や経験的知識に基づいて設定す
る。例えば、グラインダによる研磨作業の場合ではＫHE
ND＝０（Ｎ／ｃｍ）などに設定する。

【００９６】そして、各制御サイクルごとに、その制御
サイクルにおける仮想ばね定数の初期値Ｋ(n) と仮想ば
ね定数変更量ΔＫH とから次式(11)を用いて次の制御サ
イクルにおける仮想ばね定数ＫH(n+1)を求める（Ｓ２
２）。

【００９７】 ＫH(n+1)＝ＫH(n)＋ΔＫH ・・・(11) これを変更サイクル数であるＮ回だけ繰り返す。本実施
例では、このようにして、マニピュレータの仮想ばね定
数を最終仮想ばね定数ＫHENDまで、仮想ばね定数変更時
間Ｔw かけて徐々に変更する。

【００９８】手先力可変ゲイン演算手段５、指令力可変
ゲイン演算手段８、及び位置偏差トルク変換手段１３
は、各制御サイクルごとに次制御サイクルの仮想ばね定
数ＫH(n+1)に基づいてゲインを調整し、マニピュレータ
の制御を行う（Ｓ２４）。

【００９９】このように本実施例によれば、接触判定の
信頼性が向上し、位置制御から力制御への制御モードの
切換えを安定して行うことができる。また、力しきい値
Ｆthが最適値ではなく多少大まかに設定された場合で
も、信頼性の高い接触判定が行える。さらに、制御モー
ドの切換えを仮想ばね定数の変更で行っているので、マ
ニピュレータの速度の突変がなく、制御モードの切換え
を滑らかに行うことができる。

【０１００】なお、ここでは、仮想ばね定数変更量ΔＫ
H として式(10)から求められた固定値を用いたが、別の
方法として、各制御サイクルごとに仮想ばね定数の現在
値を検出し、その現在値と最終仮想ばね定数とその制御
サイクルにおける変更サイクル数の残り回数とから仮想
ばね定数変更量を求める構成としても良い。

【０１０１】本実施例において、仮想ばね定数変更手段
３６に、予め設定された位置指令値に基づいて仮想ばね
定数を変更する機能を付加し、ツールが対象物から離脱
する場合には位置指令値に基づいて仮想ばね定数を変更
することもできる。

【０１０２】この場合、例えば、ツールの接触作業時の
位置指令値の最終点をＰ(n) とした場合、ツールが一つ
前の通過点Ｐ(n-1) から最終点Ｐ(n) まで移動する間に
仮想ばね定数の変更を行う。このときの仮想ばね定数変
更時間Ｔw はツールの接触作業時の移動速度ｖを用い
て、次式(12)によって規定される。

【０１０３】 Ｔw ＝（Ｐ(n) −Ｐ(n-1) ）／ｖ ・・・(12) そして、仮想ばね定数変更手段３４は、式(12)で求めら
れたＴw を用いて変更サイクル数を求め、式(10)、(11)
に基づいて接触の場合と同様に仮想ばね定数を徐々に変
更する。ただし、この場合、力制御から位置制御へのモ
ード切り替えとなるので、仮想ばね定数は小さな値から
大きな値に変更する。このように離脱時においても仮想
ばね定数の変更によって制御モードを切換えることによ
り、仮想ばねが伸びきった状態から徐々に仮想ばねが縮
むように変更されるので、ツールを滑らかに対象物から
離脱させることができる。

【０１０４】なお、離脱時において、接触判定手段と反
対に検出値Ｆs が力しきい値Ｆthを下回る回数をカウン
トすることにより、ツールの離脱を判定し、離脱と判定
されたときから仮想ばね定数を変更するような構成にし
てもよい。

【０１０５】［第２実施例］次に、本発明の第２実施例
について、図３に基づいて説明する。

【０１０６】この第２実施例は、前述した第１実施例の
装置に、さらに距離センサ４Ｂと接近判定手段３８と速
度・加速度変更手段３６とを付加したものである。

【０１０７】この第２実施例において、距離センサ４Ｂ
はマニピュレータのアーム部先端のツールの近傍に装着
されている。そして、作業開始前には、距離センサ４Ｂ
の計測原点とツール端とのゼロ点調整を行う。

【０１０８】距離センサ４Ｂで検出されたツール端と対
象物との距離の検出値Ｓl は、接近判定手段３８に入力
される。

【０１０９】接近判定手段３８では、距離検出値Ｓl と
予め制御パラメータ設定手段３１で設定された距離しき
い値ｌとを次式(13)で比較する。

【０１１０】 Ｓl ≦ｌ ・・・(13) そして、接近判定手段３８は上式(13)が成立すると、ツ
ールが対象物に対して接近状態となったと判定して、接
近検出信号を発する。接近検出信号は、速度・加速度変
更手段３６及び仮想ばね定数変更手段３４に入力され
る。以下、速度・加速度変更手段３６と仮想ばね定数変
更手段３４の動作を順に説明する。

【０１１１】速度・加速度変更手段３６は、接近検出信
号を受信すると、プログラムや教示等によって予め与え
られていたマニピュレータの速度設定値を、予め制御パ
ラメータ設定手段３１で設定された減速パラメータｘ１
で除算することにより、所定の接近速度範囲まで減速変
更する。この減速パラメータｘ１は、ツールが対象物に
接触したときにオーバーシュートやくい込みがおこなら
いようにマニピュレータの速度を制御するためのもので
あり、接近状態になる前のマニピュレータの速度等を考
慮してユーザが適切な値を設定する。

【０１１２】速度・加速度変更手段３６で変更された速
度設定値は、位置指令手段１１内の速度設定部３７にお
いて新たな速度設定値として設定される。位置指令手段
１１は、速度設定手段３７で設定された新たな速度設定
値に対して積分演算を行うことにより新たな位置指令値
を生成し、その位置指令値は位置指令手段１１内の位置
指令部より出力される。そして、この位置指令値をもと
にマニピュレータの移動が制御される。

【０１１３】このようにして、接近判定手段３８で接近
状態と判定されると、マニピュレータは減速され、以後
ツールが対象物に接触するまでは低速状態で移動する。

【０１１４】そして、接触判定手段３２でツールが対象
物に接触したと判定されると、接触判定手段３２より速
度・加速度変更手段３６に対して接触検出信号が送信さ
れる。 速度・加速度変更手段３６は、接触検出信号を
受信すると、マニピュレータの接近状態時の速度設定値
に、予め制御パラメータ設定手段３１で設定された増速
パラメータｘを乗じて、所定の作業速度まで増速変更す
る。速度・加速度変更手段３６で変更された速度設定値
は、新たな速度設定値として位置指令手段１１内の速度
設定部３７に渡される。そして、位置指令手段１１は、
この速度設定値から位置指令値を生成する。この位置指
令値に基づいてマニピュレータの移動が制御される。な
お、このとき同時にマニピュレータの加速度ａも増速パ
ラメータｘを用いて上昇させてもよい。これは、例え
ば、加速度ａに対してｘ² を乗ずることによって行う。

【０１１５】このようにして、接触判定後はマニピュレ
ータの速度が、接触時の作業に適した速度まで加速され
る。

【０１１６】一方、仮想ばね定数変更手段３４は、接近
検出信号を受信すると、制御パラメータ設定手段３１で
予め設定された接触時仮想ばね定数ＫHiを目標値として
仮想ばね定数を変更を開始する。この接触時仮想ばね定
数ＫHiは、位置制御モード時の仮想ばね定数と最終仮想
ばね定数ＫHENDの中間の適切な値をユーザが予め設定し
ておく。この接近状態時の仮想ばね定数の変更では、ツ
ールが対象物に接触するときに、マニピュレータの仮想
ばね定数が接触時仮想ばね定数ＫHiになっているように
仮想ばね定数を変更する。すなわち、ツールが接近状態
となったときから対象物に接触するまでの間に、第１実
施例の場合と同様に式(9) 、(10)、(11)に従って仮想ば
ね定数の変更を行う。ただし、この場合、接触時にマニ
ピュレータの仮想ばね定数が接触時仮想ばね定数ＫHiに
なっているためには、仮想ばね定数変更時間Ｔw が距離
しきい値ｌと接近状態時のマニピュレータの速度及び加
速度とに基づいて決定される必要がある。この仮想ばね
定数変更時間Ｔw は、例えば、制御部において距離しき
い値ｌと接近状態時のマニピュレータの速度及び加速度
とから演算によって求めてもよいし、予めユーザが制御
パラメータ設定手段３１で適切な値に設定しておいても
よい。

【０１１７】このようにして、接近中に仮想ばね定数が
ＫHiまで変更される。これにより、マニピュレータの仮
想ばね定数がＫHiの状態でツールが対象物に接触するこ
とになる。接触後の仮想ばね定数の変更は、第１実施例
と同様に行う。

【０１１８】このように、本実施例によれば、ツールが
対象物に対して接触する時にはマニピュレータの速度が
低速になっており、また仮想ばね定数がある程度小さく
なっているので、接触時のマニピュレータへの衝撃や力
のオーバーシュート、及び接触時におけるツールのくい
込みを防ぐことができる。また、接触時に仮想ばね定数
がある程度まで最終仮想ばね定数ＫHENDに近付いている
ので、接触後の仮想ばね定数の変更量が少なくでき、接
触後の仮想ばね定数の変更時間を短縮することができ
る。

【０１１９】また、本実施例における接近判定手段を用
いた別の制御方法として、着地点付近に教示した通過
点、ツール端位置データ、及びパラメータ設定手段で予
め設定しておいた接近速度などを用いて、ツール端位置
と着地点の間の位置指令値を作り直し、マニピュレータ
を新たな位置指令値で接近させてもよい。

【０１２０】なお、本実施例においては、速度・加速度
変更手段３６は、接近または接触と判定されると同時に
速度及び加速度の変更を開始したが、これに限らず、例
えば、予めプログラムや教示等で設定されたマニピュレ
ータの通過点のうち、接近判定の直後及び接触判定の直
後のそれぞれの通過点から速度及び加速度の変更を開始
する構成にしてもよい。この場合は、最初に設定された
通過点に応じて速度及び加速度を変えていくことができ
るので、通過点と通過点との途中で変更を開始する場合
のようにマニピュレータの軌跡を作り直す必要がない。

【０１２１】［第３実施例］本発明の第３実施例を、図
３を参照して説明する。

【０１２２】位置指令手段１１は、プログラムや教示等
に基づいてマニピュレータの軌跡を表す一連の位置指令
値、及びそれら位置指令値に対応した速度設定値をマニ
ピュレータに対して指令する。また、位置指令手段１１
は、一連の位置指令値のうち、マニピュレータが対象物
に向かって移動する途中の、ある所定の位置からの速度
設定値を所定の接近速度に設定する。この接近速度は、
ツールが対象物に接触したときに力のオーバーシュート
やツールのくい込みが起こらない程度の速度である。接
近速度の値はマニピュレータの特性や行う作業の種類に
よって異なる。従って、マニピュレータは、対象物に向
かって移動する時、途中のある所定の位置にくると接近
速度まで減速されることになる。そして、接触判定手段
３２で接触と判定されるまでは、マニピュレータは、そ
の接近速度で移動する。このとき、マニピュレータは位
置制御により、その接近速度でゆっくりと対象物に近付
いていく。

【０１２３】この時、常時各サイクルごとに、６軸力セ
ンサ４Ａによりツール部に作用する力等の検出値Ｆs が
検出され、その検出値Ｆs が接触判定手段３２において
力しきい値Ｆthと上式(7) で比較される。以下、接触判
定手段３２は、第１実施例と同様にしてツールの接触判
定を行う。そして、ツールが対象物に接触したと判定す
ると、接触判定手段３２は、接触検出信号を速度・加速
度変更手段３６に送信する。

【０１２４】速度・加速度変更手段３６は、接触検出信
号を受信すると、マニピュレータの速度を作業時に必要
な速度まで上昇させる。すなわち、速度・加速度変更手
段３６は、マニピュレータの接近速度に予め制御パラメ
ータ設定手段３１で設定された増速パラメータｘを乗じ
て、所定の作業速度まで速度設定値を増速変更する。こ
の増速パラメータｘは、マニピュレータの接近速度と作
業速度とを考慮して、ユーザが設定する。

【０１２５】速度・加速度変更手段３６で変更された速
度設定値は、新たな速度設定値として位置指令手段１１
内の速度設定部３７に渡される。そして、位置指令手段
１１は、この速度設定値から位置指令値を生成する。こ
の位置指令値に基づいてマニピュレータの移動が制御さ
れる。なお、このとき同時にマニピュレータの加速度ａ
も増速パラメータｘを用いて上昇させてもよい。これ
は、例えば、加速度ａに対してｘ² を乗ずることによっ
て行う。

【０１２６】このようにして、マニピュレータは対象物
に接触すると所望の作業速度で作業を行うことができ
る。

【０１２７】本実施例では、さらに作業が終了してツー
ルが対象物から離脱する際においてもマニピュレータの
速度制御を行う。この速度制御は以下のようにして行わ
れる。 この場合、接触判定手段３２における判定式を
式(7) の代わりに次式(14)を用いることにより、接触判
定手段３２を離脱判定手段として用いる。

【０１２８】 Ｆs ≦Ｆth´ ・・・(14) ここで、Ｆs は６軸力センサ４Ａから得られた検出値で
あり、Ｆth´は離脱判定のための力しきい値である。な
お、Ｆth´としては接触判定のときの力しきい値Ｆthと
同じ値を用いてもよい。

【０１２９】そして、制御サイクルごとに上式(14)の判
定を行い、上式(14)がｍ´制御サイクル以上連続して成
立した場合にツールが対象物から離脱したと判定し、速
度・加速度変更手段３６に対し離脱検出信号を送信す
る。ここで、ｍ´は予め制御パラメータ設定手段３１で
ユーザが設定するものであり、第１実施例で接触判定の
際に用いたｍと同じ値でもよい。

【０１３０】速度・加速度変更手段３６は、離脱検出信
号を受信すると、マニピュレータの作業時の速度設定値
に、予め制御パラメータ設定手段３１で設定された増速
パラメータｘ´を乗じて増速変更する。速度・加速度変
更手段３６で変更された速度設定値は、新たな速度設定
値として位置指令手段１１内の速度設定部３７に渡され
る。そして、位置指令手段１１は、この速度設定値から
位置指令値を生成する。この位置指令値に基づいてマニ
ピュレータの移動が制御される。なお、このとき同時に
マニピュレータの加速度ａも増速パラメータｘ´を用い
て上昇させてもよい。これは、例えば、加速度ａに対し
てｘ´² を乗ずることによって行う。

【０１３１】このように、本実施例によれば、所定の作
業速度を満足しつつ接触時のマニピュレータの速度を小
さくできるので、ツールを対象物に対して緩やかに接触
させることができ、ツールのくい込みや力のオーバーシ
ュートなどを防ぐことができる。また、ツールの離脱が
検出されると同時にマニピュレータの速度を増速するこ
とにより、マニピュレータの一連の作業サイクルを短縮
することができる。

【０１３２】なお、本実施例では、速度・加速度変更手
段３６によるマニピュレータの速度及び加速度の変更
は、接触判定手段３２によって接触と判定されると同時
に開始されたが、これに限らず、例えば、第２実施例の
場合と同様に、予めプログラムや教示等で設定されたマ
ニピュレータの通過点のうち、接近判定の直後及び接触
判定の直後のそれぞれの通過点から速度及び加速度の変
更を開始する構成にしてもよい。好ましくは、対象面の
やや内側に着地後最初に通過する通過点を設けることに
よって、制御モード切り換え後、素早く速度・加速度の
変更ができ、種々の作業において作業速度を満足するこ
とができる。この場合は、最初に設定された通過点に応
じて速度及び加速度を変えていくことができるので、通
過点と通過点との途中で変更を開始する場合のようにマ
ニピュレータの軌跡を作り直す必要がない。

【図面の簡単な説明】

【図１】本発明の原理構成図である。

【図２】本発明に係るマニピュレータ制御装置の構成を
示すブロック図である。

【図３】本発明の実施例を示すブロック図である。

【図４】本発明の実施例における処理過程を示すフロー
チャートである。

【図５】従来のマニピュレータ制御装置の構成を示すブ
ロック図である。

【符号の説明】

Ａ 力検出手段 Ｂ 接触判定手段 Ｃ 変更サイクル数計算手段 Ｄ 仮想ばね定数変更手段 Ｅ マニピュレータ Ｆ 接近判定手段 Ｇ 速度・加速度変更手段

Claims (5)

【特許請求の範囲】
1. 【請求項１】 ツールを対象物に接触させて所望の作業
   を行うマニピュレータを制御するマニピュレータ制御装
   置であって、 各制御サイクルごとに前記ツールが外部環境から受ける
   力及びモーメントのうち少なくとも一方を検出する力検
   出手段と、 各制御サイクルごとに前記力検出手段の検出値を力しき
   い値と比較し、前記検出値が予め指定された制御サイク
   ル数以上連続して前記力しきい値以上となった場合に前
   記ツールが対象物に接触した判定する接触判定手段と、 予め指定された仮想ばね定数変更時間と所定の制御サイ
   クル時間とに基づいて、マニピュレータの仮想ばね定数
   を変更するときの変更サイクル数を算出する変更サイク
   ル数計算手段と、 前記接触判定手段によって接触と判定された時点から、
   マニピュレータの仮想ばね定数を予め指定された最終仮
   想ばね定数に前記変更サイクル数で変更する仮想ばね定
   数変更手段と、 を含むことを特徴とするマニピュレータ制御装置。
2. 【請求項２】 請求項１記載のマニピュレータ制御装置
   であって、 さらに、前記ツールと前記対象物との距離を検出する距
   離検出手段と、 検出された距離と予め指定された距離しきい値とを比較
   し、前記距離が前記距離しきい値以下になったときに接
   近状態と判定する接近判定手段と、 前記接近判定手段及び接触判定手段のうち少なくとも一
   方の判定結果に基づいてマニピュレータの速度及び加速
   度を変更する速度・加速度変更手段と、 を含み、 前記速度・加速度変更手段は、前記接近判定手段により
   接近状態と判定されるとマニピュレータの速度を所定の
   接近速度に変更し、前記接触判定手段により接触と判定
   されるとマニピュレータの速度を所定の作業速度に変更
   することを特徴とするマニピュレータ制御装置。
3. 【請求項３】 請求項２記載のマニピュレータ制御装置
   において、 前記仮想ばね定数変更手段は、前記接近判定手段により
   接近状態と判定された時点から前記接触判定手段により
   接触と判定されるまでの間に、マニピュレータの仮想ば
   ね定数を予め指定された接触時仮想ばね定数に変更する
   ことを特徴とするマニピュレータ制御装置。
4. 【請求項４】 ツールを対象物に接触させて所望の作業
   を行うマニピュレータを制御する装置であって、 マニピュレータの軌跡を表す一連の位置指令値及びその
   位置指令値に対応した速度設定値を出力するとともに、
   前記一連の位置指令値のうち所定の位置指令値以降は前
   記位置指令値に対応した速度設定値として所定の接近速
   度を出力する位置指令手段と、 各制御サイクルごとに前記ツールが外部環境から受ける
   力及びモーメントのうち少なくとも一方を検出する力検
   出手段と、 各制御サイクルごとに前記力検出手段の検出値を予め指
   定された力しきい値と比較し、前記検出値が予め指定さ
   れた制御サイクル数以上連続して前記力しきい値以上と
   なったときに前記ツールが対象物に接触した判定する接
   触判定手段と、 前記接触判定手段の判定結果に基づいてマニピュレータ
   の速度及び加速度を変更する速度・加速度変更手段と、 を含み、 前記速度・加速度変更手段は、前記接触判定手段でツー
   ルが対象物に接触したと判定されるとマニピュレータの
   速度を予め指定された作業速度に変更することを特徴と
   するマニピュレータ制御装置。
5. 【請求項５】 請求項４記載のマニピュレータ制御装置
   であって、 さらに、各制御サイクルごとに前記力検出手段の検出値
   を予め指定された第２の力しきい値と比較し、前記検出
   値が予め指定された制御サイクル数以上連続して前記第
   ２の力しきい値以下となったときに、ツールが対象物か
   ら離脱したと判定する離脱判定手段を含み、 前記速度・加速度変更手段は、さらに、前記離脱判定手
   段でツールが対象物から離脱したと判定されると、マニ
   ピュレータの速度を所定の離脱速度まで上昇させること
   を特徴とするマニピュレータ制御装置。