(57)【要約】【課題】繰り返し試行する時間を省くことで力制御ゲインの設定及び調整を容易に行うことができる制御装置を提供する。【解決手段】力制御回路７の内側に位置制御回路８を有する力制御システムによりロボット１を力制御する制御装置において、ロボット１を駆動する実際の制御条件で、ロボット１の個々の駆動軸に関する位置制御回路８の時定数Ｔを取得する時定数取得手段１１と、ロボット１を駆動する実際の制御条件で、ロボット１の剛性値Ｋを取得する剛性値取得手段１２と、力制御回路７の時定数が位置制御回路８の時定数Ｔより大きくなる条件で、時定数取得手段１１で取得された位置制御回路８の時定数Ｔと剛性値取得手段１２で取得された剛性値Ｋとから力制御回路７の力制御ゲインを算出するゲイン自動算出手段１０と、を備える。【選択図】図２
2007237312.tif 000002
【特許請求の範囲】
【請求項１】
力制御回路の内側に位置制御回路を有して制御対象を力制御する制御装置において、
前記制御対象を駆動する実際の制御条件で、前記制御対象の個々の駆動軸に関する前記位置制御回路の時定数を取得する第１のデータ取得手段と、
前記制御条件で、前記制御対象が前記ワークに接触したときの力データに基づいて、前記制御対象とワークの剛性値を取得する第２のデータ取得手段と、
前記力制御回路の時定数が前記位置制御回路の時定数より大きくなる条件で、前記第１のデータ取得手段で取得された前記位置制御回路の時定数と、前記第２のデータ取得手段で取得された前記剛性値とから、力制御回路の力制御ゲインを算出するゲイン自動算出手段と、
を備えたことを特徴とする制御装置。
【請求項２】
前記力データが、前記ワークに対する前記制御対象の接触部近傍に設けられた力センサで検出された請求項１記載の制御装置。
【請求項３】
前記力データが、前記制御対象を駆動するアクチュエータの電流値から取得された請求項１記載の制御装置。
【請求項４】
前記力制御がダンピング制御である請求項１〜３の何れか１項に記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば組み立てや研磨加工などを行う制御対象としての産業用ロボットや工作機械などに適用され、力制御回路の内側に位置制御回路を有して制御対象を力制御する制御装置に関する。
【背景技術】
【０００２】
制御対象を力制御する制御装置は、力制御回路の力制御ゲインが設定・調整された後に、産業用ロボットなどを制御するために用いられる。力制御される産業用ロボットは、精密嵌合部品の嵌め合わせやギアの位相合わせなどの組み立て、グライダを用いた研磨などの接触を伴う精密な作業を行うことができる。ロボットにこのような精密な作業を短いサイクルタイムで安定して行わせるには、力制御回路の力制御ゲインを適切に設定・調整する必要がある。
【０００３】
力制御ゲインは、ロボット先端に装着したハンドツールの質量・イナーシャ、ロボット・ハンドツール・対象物（対象ワーク）のバネ定数、作業時のロボット姿勢などの実際の作業環境に影響されるため、力制御をする方向（個々の駆動軸方向）ごとに一定力をかけ、十分低いゲインから少しずつゲインを上げながら力指令値に対するオーバーシュート量を検出し、オーバーシュート量が一定量を越えない範囲で力制御ゲインの上限値を得る必要がある。しかし、この方法は制約が多く時間がかかり、熟練を要するという問題がある。
【０００４】
一方、力制御ゲインを自動で設定することも可能である。自動で設定する場合は、プログラムで少しずつ力制御ゲインを増やしながら作業を行い、その際の力の波形を記録し、立ち上がり時間やオーバーシュート量、振動成分などを計算して適切な力制御ゲインを計算する。しかし、この方法では、調整する際の力制御ゲインの初期値に気を配る必要があり、適切な力制御ゲインより高い力制御ゲインを設定した場合には、最初の試行で力制御が発振してしまい、作業を行うロボットや機械や、機械に付けたハンドツール、対象物等を壊してしまう可能性があった。逆に、これを避けるために低い値の力制御ゲインから調整を始めると、試行を繰り返す回数が増え、調整に時間がかかるという問題があった。さらに、多軸ロボットの場合、力制御が行われる空間は多くの場合、並進３方向とその軸回りの回転３方向を合わせた６軸の空間である場合が多いため、その軸毎に力制御ゲインを設定すると、ゲインの設定作業に非常に多くの時間を要することとなっていた。
【０００５】
また、力制御ゲインの調整を行う際には、その全ての軸方向について、対象ワークとの間に接触が生じる様な環境を用意してやらなければならない。何故ならば、ある軸方向にはたまたま接触する機会が無かったときに得られた力制御ゲインの値をそのまま使うと、接触したときに不安定になる可能性があるためである。例えば、図６の様にキー付き棒４１を穴４２に挿入する際、図示されるように棒４１と穴４２とがずれた状態で接触出来る方向は挿入方向のＺ方向だけであるから、Ｘ、Ｙ方向では接触が出来ず、穴４２に棒４１が挿入された後でないと、Ｘ、Ｙ方向の力制御ゲインの調整を行うことができないこととなる。
【０００６】
また、他の例として、円筒形の対象ワークを穴に入れる作業でその隙間が非常に小さい場合などは、低い値の力制御ゲインから始めたのでは力制御の性能が悪いために挿入そのものができないという不都合が生じる。このように、力制御ゲインの調整初期段階では、力制御ゲインの性能が悪いにも拘わらず、二つの部品を嵌合させなければならない場合があり、力制御ゲインの設定又は調整する作業自体に矛盾を生じていた。
【０００７】
また、制御方法の他の従来例として、種々の作業状態に応じて力制御ゲインを適切に調整して制御対象を制御するものではないが、多数の動作修正モジュールを有し、各作業状態に応じて動作モードを切り替えるものが開示されている。
【０００８】
【特許文献１】特開平６−３２８３７９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
本発明は、上記した点に鑑み、繰り返し試行する時間を省くことで力制御ゲインの設定及び調整を容易に行うことができ、ゲイン調整の初期段階において”ゲインを調整するためにはまずは挿入しなければならず、挿入するためにはある程度の高いゲインを設定する必要がある”という矛盾を抱えた問題を回避することができる制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記目的を達成するために、請求項１記載の制御装置は、力制御回路の内側に位置制御回路を有して制御対象を力制御する制御装置において、前記制御対象を駆動する実際の制御条件で、前記制御対象の個々の駆動軸に関する前記位置制御回路の時定数を取得する第１のデータ取得手段と、前記制御条件で、前記制御対象が前記ワークに接触したときの力データに基づいて、前記制御対象とワークの剛性値を取得する第２のデータ取得手段と、前記力制御回路の時定数が前記位置制御回路の時定数より大きくなる条件で、前記第１のデータ取得手段で取得された前記位置制御回路の時定数と、前記第２のデータ取得手段で取得された前記剛性値とから、力制御回路の力制御ゲインを算出するゲイン自動算出手段と、を備えたことを特徴とする。
【００１１】
また、請求項２記載の発明は、請求項１に記載の制御装置において、前記力データが、前記ワークに対する前記制御対象の接触部近傍に設けられた力センサで検出される。
また、請求項３記載の発明は、請求項１に記載の制御装置において、前記力データが前記制御対象を駆動するアクチュエータの電流値から取得される。
また、請求項４記載の発明は、請求項１〜３の何れか１項に記載の制御装置において、前記力制御がダンピング制御である。
【発明の効果】
【００１２】
以上の如く、請求項１記載の発明によれば、第１のデータ取手段により位置制御回路の時定数が取得され、第２のデータ取得手段により制御対象の剛性値が取得され、ゲイン自動算出手段により、制御回路の時定数と制御対象の剛性値から力制御ゲインが算出されるから、従来のように、ゲインの設定作業に時間と手間をかけることなく、力制御ゲインの設定及び調整を容易に行うことができ、ゲイン調整の初期段階において”ゲインを調整するためにはまずは挿入しなければならず、挿入するためにはある程度の高いゲインを設定する必要がある”という矛盾を抱えた問題を回避することができる。また、ロボットや機械の据付や作業プログラムの変化（作業工程の変化など）にフレキシブルに対応することができ、汎用性の高い制御装置を提供することができる。
【００１３】
また、請求項２記載の発明によれば、力データが制御対象の接触部近傍に設けられた力センサで検出されることで、精度の高い剛性値を推定することができる。
【００１４】
また、請求項３記載の発明によれば、力データがアクチュエータの電流値から取得されることで、コストをかけずに簡易な方法で剛性値を推定することができる。
【００１５】
請求項４記載の発明によれば、世の中で広く使われているダンピング制御に本発明を適用する事が出来、ダンピング制御のゲイン調整の手間がかからず、より手軽にダンピング制御を利用する事が出来る様になる。
【発明を実施するための最良の形態】
【００１６】
以下、本発明に係るロボット制御装置（制御装置）を図面に基づいて説明する。図１には制御対象としての並進３軸、回転３軸の６軸ロボットが示されている。図示するロボットなど力制御する制御装置は、力制御回路の内側で直列に位置制御回路を持つシステムと、力制御回路の外側で並列に位置制御回路を持つシステムとに大別される。本発明に係る制御装置（図２）は、力制御回路の内側で直列に位置制御回路を持つシステム（図３参照）を対象とし、ロボットと対象ワークの接触状態を力制御すると共に、位置制御回路の時定数（遅れ）とロボット及び対象ワークの組み合わせ剛性（制御系によって、ロボットの剛性に置き換えることも可能である）とから、力制御ゲインを自動的に設定することができるものである。
【００１７】
ここで、推定される剛性には、ロボット及び対象ワークを含めた剛性が用いられているが、近年の多くのロボットや、高速化・軽量化が重視された機械は、接触対象物のワークに比べてロボットや機械側の剛性が低い場合が大半となり、力制御システムに与える強度の影響は機械側の剛性が支配的となっているから、接触対象の剛性をロボットと接触対象を含めた剛性に置き換えても問題はない。
【００１８】
図１に示されるロボットは、嵌合部品の嵌合作業を行うものである。このロボットは、ケーブルＣＢ１を用いてロボット制御装置５に接続され、その手先部２には力センサ３が取り付けられている。力センサ３は、歪ゲージを含むブリッジ回路などで構成され、力センサ３の検出部に作用する６軸方向の力を検出し、ロボット１の力制御のために、検出した力データをケーブルＣＢ２を通してロボット制御装置５に送信する。力センサ３に装着／固定されたハンドツール４は、適当な位置で把持爪６の開閉動作を行なって対象ワーク２０を把持する。
【００１９】
本実施形態の対象ワーク２０は、凸部２１と底面２２とを有する二段円筒形状の組立部品として示されている。一方、嵌合相手の相手ワーク２５は、凹部２６を有する円筒部２７が形成された組立部品であり、作業テーブル３０上に設置された位置決め装置３２上に供給され、所定位置で位置決めされる。凹部２６は、円筒部２７と同心に形成され、対象ワーク２０の凸部２１をタイトな関係で受け入れるサイズを有している。相手ワーク２５は、ワーク基準の独自の座標系（ワーク座標系）を有しており、ロボット制御装置５のメモリに記憶されている。ロボット１も、ロボット１基準の独自の座標系（ロボット座標系）を有しており、ロボット制御装置５のメモリに記憶されている。ロボット制御装置５は、個々の座標系におけるワーク２５とロボット１の個々の座標を統一した座標系に変換し直して、両ワーク２０，２５の嵌合を確実に行うようにする。
【００２０】
図２に示されるように、ロボット制御装置５は、作成した力制御の指令値を出力する計算機６と、力指令値を力検出器で検出された実際の力データに基づいて修正処理する力制御回路７と、力制御回路７の内側に位置して速度指令を出力する位置制御回路８と、位置制御回路７の内側に位置してトルク指令値を出力する図示しない速度制御回路と、トルク指令値を電流指令値に変換し、図示しないサーボアンプを介してサーボモータ１４を制御する図示しない電流制御回路とから構成されている。力制御回路７と、位置制御回路８と、速度制御回路と、電流制御回路は、ロボット１の各軸を動かすデジタルサーボ回路を構成している。なお、本実施形態では、力検出器としての力センサ３や、位置・速度検出器としてのエンコーダ１６や、ＣＣＤカメラなどはロボット制御装置５に含まれていないが、これらを制御装置に含めることもできる。
【００２１】
計算機６は、ＣＰＵ（中央演算処理装置）を備えている。ＣＰＵには、ＲＯＭからなるメモリ、ＲＡＭからなるメモリ、不揮発性メモリ、液晶ディスプレイを備えた教示操作盤、各種インターフェース、各種メモリがバスを介して接続されている。インターフェースには、力センサ３や、ディスプレイモニタや、ＣＣＤカメラなどを接続することができる。
【００２２】
力制御回路７は、対象ワーク２０を相手ワーク２５に嵌合させる実際の制御条件でロボット１を動作させたときの、ロボット１の個々の駆動軸に関する位置制御回路８の時定数Ｔを推定する時定数取得手段（第１のデータ取得手段）１１と、ロボット１のハンドツール４に把持された対象ワーク２０を相手ワーク２５に接触させたときのロボット１の剛性値Ｋを推定する剛性値取得手段（第２のデータ取得手段）１２と、力制御回路７の時定数が位置制御回路８の時定数Ｔより大きくなる条件で、時定数取得手段１１で推定された時定数Ｔと剛性値取得手段１２で推定された剛性値Ｋとから力制御回路７の力制御ゲインＧを算出するゲイン算出手段１０とを備えている。
【００２３】
時定数取得手段１１は、詳細は後述するように、ロボット１に対する位置指令と、実際のロボット位置のデータとを用い、適応フィルタで位置制御回路８の時定数Ｔを推定する。推定された時定数Ｔは、ロボット１の位置・姿勢や、ワーク２０，２５の慣性などを含む環境状態によって変化する。力制御ゲインＧは、ロボット１の位置・姿勢などによって変化する時定数の影響を受けるものとして求められることとなる。
【００２４】
剛性値取得手段１２は、ロボット１のハンドツール４に把持された対象ワーク２０を相手ワーク２５に接触させたときの力データと、接触時のロボット１の位置データとを用い、適応フィルタで剛性値を推定する（このような剛性値は、ロボット周囲の環境状態によって決まるため、環境剛性ということもある）。
【００２５】
ここで、力制御を行うロボット１などで力を検知する方法には、手先部２に取り付けられた力センサ３を使う方法や、ロボット１の各軸に備えられたトルクセンサのトルク値から対象物にかかる力を推定する方法や、ロボット１の各軸モータの電流値から対象物にかかる力を推定する方法などがあるが、本実施形態では力センサ３が用いられている。力センサ３で検出される力は、フックの法則により、ロボット１とワーク２０，２５間の変位量と、ロボット１とワーク２０，２５間の剛性Ｋとの積に等しくなる。力センサ３が用いられることで、精度の高い剛性値Ｋを推定することができる。
【００２６】
なお、力データがロボット１の各軸に備えられたトルクセンサのトルク値から推定される方法や、モータの電流値から推定される方法を採ってもよい。
【００２７】
次に、力制御ゲインを自動調整する方法について説明する。図４にはフローチャートが示されており、ステップＳ１でロボット１が作業点近傍の位置へ移動し、ステップＳ２で駆動軸毎にロボット１を往復動作させたときの位置制御回路８の時定数Ｔが推定され、ステップＳ３で相嵌合するワーク２０，２５が接触したときのロボット１とワーク２０，２５間の剛性Ｋが推定された後、位置制御回路８の時定数Ｔとロボット１とワーク２０，２５間の剛性Ｋとに基づいて力制御ゲインＧが自動的に設定される。以下において、位置制御回路８の時定数Ｔの推定方法、剛性Ｋの推定方法、力制御ゲインＧの算出方法をについて説明する。
【００２８】
位置制御回路８の時定数Ｔの推定方法は、力制御を行うロボット１の全ての関係する軸方向に対して、それぞれ往復運動をさせ、その際の指令位置と実際の位置から、位置制御回路８の遅れ時間に相当する時定数Ｔを推定する。具体的には、位置制御回路８を１次遅れ系のモデル（１／(Ｔ・ｓ＋１））と仮定して、Ｔを推定する。
【００２９】
ｕ（ｋ）：指令位置、ｙ（ｋ）：実位置、Δｔ：位置制御回路の計算周期、として、離散値形で適応フィルタの数式を作ると、以下のようになる。
【００３０】
【数１】
2007237312.tif 000003
【００３１】
ここで、ｕ（ｋ）−ｙ（ｋ）は入力値、ｙ（ｋ）−ｙ（ｋ−１）は出力値を意味する。このモデルを使って、Δｔ／Ｔを推定するための漸化式を作ると、以下のようになる。
【００３２】
【数２】
2007237312.tif 000004
【数３】
2007237312.tif 000005
【００３３】
ここで、εｐ（ｋ）は誤差を意味し、θｐ（ｋ）は推定値を意味する。Δｔ／Ｔに収束する最終的なθp(k)の値から時定数Ｔを求めると、以下のようになる。
【００３４】
【数４】
2007237312.tif 000006
【００３５】
剛性の推定方法では、その時点の制御周期での剛性をＫ、力データをＦ（ｋ）、実位置をｙ（ｋ）、平衡位置補正用パラメータ（オフセット）をＦ０として、力が変位と剛性の積に等しくなるモデル式を作ると、以下のようになる。
【００３６】
【数５】
2007237312.tif 000007
【００３７】
【数６】
2007237312.tif 000008
【００３８】
上記モデル式から、適応フィルタで剛性Ｋを推定すると、漸化式は以下のようになる。
【００３９】
【数７】
2007237312.tif 000009
【数８】
2007237312.tif 000010
【００４０】
漸化式で更新していくと、（２行１列）の列ベクトルθｋ（ｋ）が［Ｋ  Ｆ０］Tに収束する。θｋ（ｋ）の第１要素の値をもって剛性Ｋとする。
【００４１】
次に、求めた剛性Ｋと位置制御回路８の時定数Ｔから、力制御ゲインＧを求める方法について述べる。図３の力制御回路において、力制御回路７の時定数が位置制御回路８の時定数Ｔより十分に大きい場合、図５の様な力制御回路７であるとみなせる。この時、力指令Ｆｄから力Ｆまでの伝達関数は、以下の式で表される。
【００４２】
【数９】
2007237312.tif 000011
【００４３】
これは、時定数が（１／（Ｇ・Ｋ））の１次遅れ要素としてみなす事が出来る。従って、力制御回路７の時定数が位置制御回路８の時定数Ｔより十分大きいある時間、例えば１０Ｔなら安定になる場合、Ｇを以下のようにする。
【００４４】
【数１０】
2007237312.tif 000012
【００４５】
力制御の応答性をなるべく速くするためには、以下のようにすることができる。
【００４６】
【数１１】
2007237312.tif 000013
【００４７】
以上のように、本実施形態によれば、力制御ゲインＧの設定作業に時間と手間をかけることなく、力制御ゲインＧの設定を容易に行うことができる。また、ロボット１や機械の据付や作業プログラムの変化（作業工程の変化など）にフレキシブルに対応することができ、汎用性の高いロボット制御装置５を提供することができる。また、時定数Ｔや剛性Ｋに推定値を使用することで、ノイズによる影響を低減することもできる。
【００４８】
なお、本発明は上記実施形態に限定されるものではなく、本発明の骨子を逸脱しない範囲で種々変形して実施することができる。例えば、本実施形態では、位置制御回路８の時定数Ｔ及びロボット１の剛性Ｋには適応フィルタで推定された推定値が用いられているが、推定値に代えてそれらを実際に計測した計測値を用いることもできる。また、適応フィルタは使用せずに、カルマンフィルタやＨ∞フィルタ等、他の推定器を使う事もできる。
【図面の簡単な説明】
【００４９】
【図１】本発明の制御装置を含むロボット制御システムの一実施形態を示す斜視図である。
【図２】図１に示す制御装置の構成図である。
【図３】同じく制御装置の制御信号の流れを示すブロック図である。
【図４】力制御ゲインの算出方法を説明するためのフローチャートである。
【図５】力制御回路を一次遅れモデルとみなしたブロック図である。
【図６】キー付き棒を穴に挿入する状態を示す説明図である。
【符号の説明】
【００５０】
１    ロボット
３    力センサ
５    ロボット制御装置（制御装置）
７    力制御回路（力制御ループ）
８    位置制御回路（位置制御ループ）
１０    ゲイン算出手段
１１    時定数取得手段（第１のデータ取得手段）
１２    剛性値取得手段（第２のデータ取得手段）
２０，２５    ワーク
Ｔ    時定数
Ｋ    剛性
Ｇ    力制御ゲイン
【図１】
2007237312.tif 000014
【図２】
2007237312.tif 000015
【図３】
2007237312.tif 000016
【図４】
2007237312.tif 000017
【図５】
2007237312.tif 000018
【図６】
2007237312.tif 000019