JP2011088225A - インピーダンス制御パラメータの調整装置および調整方法 - Google Patents

Abstract

【課題】 インピーダンス制御の複数のパラメータを個人のスキルに依存することなく、特別な知識を有していなくても、常に最適なパラメータを調整できるようにする。
【解決手段】 パラメータ初期値算出部１１７と力指令印加部１１１と評価基準計測部１１２と許容値設定部１１３と粘性パラメータ探索部１１４と終了判断部１１５と慣性パラメータ調整部１１６を備え、力指令印加部１１はインピーダンス制御部に対して力指令を入り切りし、評価基準計測部１１２は力応答の整定時間とオーバシュート量と振動回数を計測し、許容値設定部１１３はオーバシュート量と整定時間の許容値を設定し、粘性パラメータ探索部１１４は整定時間が最小となる粘性パラメータを探索し、終了判断部１１５は調整値と許容値を比較して処理の終了あるいは継続を判断し、慣性パラメータ調整部１１６はオーバシュート量と整定時間の調整値に応じて慣性パラメータを調整する。
【選択図】 図１

Description

本発明は、産業用ロボットのエンドエフェクタに設けた力センサ情報に基づくインピーダンス制御のパラメータ調整装置、およびパラメータ調整方法に関する。
インピーダンス制御とは、ロボットの手先に外から力を加えた場合に生じる機械的なインピーダンス（慣性、減衰係数、剛性）を、目的とする作業に都合の良い値に設定するための位置と力の制御手法のことである。
ロボットの手先にバネやダンパなどの機械要素を取り付けて手先のインピーダンスを変更する受動インピーダンス法と、手先の位置、速度、力などの測定値を用いたフィードバック制御でインピーダンスを変更する能動インピーダンス法がある。

産業用ロボットで嵌合など接触作業を実行する場合、ロボットマニピュレータのエンドエフェクタまたは手首部に力センサを設け、力センサ情報を利用した力制御を構成する技術が広く知られている。具体的な力制御手法として、例えば、インピーダンス制御が利用しやすい。
インピーダンス制御は、エンドエフェクタに外力が作用した際の位置の応答が、望みの慣性（マス）、粘性（ダンパ）、剛性（バネ）特性にしたがって動作するようにフィードバック制御を構成するものである。嵌合作業時にワーク同士が接触したときの力を受け流して位置誤差を吸収することができる。ただし、慣性、粘性、剛性のパラメータが不適切であると、作業実行に極端に時間がかかる場合がある。また、接触時に制御系が不安定になり（発振し）、作業が遂行できないだけでなく、ワークやロボットを破損させる危険性もある。そのため、インピーダンス制御のパラメータを適切に調整する必要がある。
インピーダンス制御のパラメータを調整する技術として、特許文献１が開示されている。
特許文献１では、調整作業者（教示者）が力応答を表示部で確認しながら、重さ（重い⇔軽い）および硬さ（硬い⇔軟らかい）の挙動指定パラメータをボタン操作で調整し、その挙動指定パラメータを基に、ファジ推論によりインピーダンスパラメータを調整している。

特開２００１−２７７１６２

特許文献１では、力応答を目視で確認しながらボタン操作でパラメータを調整している。特許文献１では、２つの挙動指定パラメータで３つのインピーダンスパラメータを調整する、ファジ推論によって調整する、という工夫が見られるが、ファジ推論は個人の主観的判断を統計的に処理するためのものであり、個人差を無くすためのものではない。したがって、調整結果や調整時間が個人のスキルに依存するという問題がある。調整結果が個人差に依存するので最適なパラメータ調整ができるとは言い難い。

本発明はこのような問題点に鑑みてなされたものであり、インピーダンス制御の複数のパラメータを個人のスキルに依存することなく、特別な知識を有していなくても、常に最適なパラメータを調整する装置および調整方法を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
本発明はロボットマニピュレータのエンドエフェクタに設けた力センサ情報を基に構成されたインピーダンス制御のパラメータを調整する装置であって、
パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値を算出するパラメータの初期値算出部と、
インピーダンス制御へステップ状の力指令を入り切りすることで、ハンドエフェクタが把持したワークを対象ワークに繰り返し押し当てる力指令印加部と、
押し当ての度に力センサからの力フィードバックの時間応答を記録するとともに、力フィードバックの時間応答の整定時間とオーバシュート量と振動回数を自動計測する評価基準計測部と、
許容できるオーバシュート量の最大値であるオーバシュート量許容値と、許容できる整定時間の最大値である整定時間許容値を設定する許容値設定部と、
慣性パラメータを固定し、繰り返し押し当てを実行して整定時間が最小になるような粘性パラメータを探索する粘性パラメータ探索部と、
粘性パラメータ探索部による粘性パラメータ探索の結果、評価値計測部から得られる整定時間調整値とオーバシュート量調整値を、前記許容値と比較することによって繰り返し処理を終了するか、継続するかを判断する終了判断部と、
終了判断部が処理を継続すると判断した場合に、慣性パラメータを増減するか、オーバシュート量許容値と整定時間許容値のいずれか一方を緩和した後、粘性パラメータ探索部を再実行する慣性パラメータ調整部と、
を備えたインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記評価基準計測部は、ステップ状の前記力指令値を中心に上限および下限を指定した整定範囲でもって整定時間を計測することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記評価基準計測部は、力フィードバック応答が前記整定範囲を上回った回数と下回った回数を交互に計測し、その回数の和でもって振動回数を計測することを特徴とする請求項２に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記粘性パラメータ探索部は、前記振動回数が予め設定した第１の閾値を上回る場合に粘性パラメータを増加させ、予め設定した第２の閾値を下回る場合に粘性パラメータを減少させることを特徴とする請求項３に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記粘性パラメータ探索部は、前記振動回数が前記第２の閾値以上で前記第１の閾値以下であるとき、整定時間が前回に比べて増加した場合に、粘性パラメータの探索方向を反転させるとともに、探索幅を予め設定した比率で減少させ、整定時間の前回からの変化量が予め設定した閾値以下になったときに探索処理を終了することを特徴とする請求項３または４に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記終了判断部は、オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値以下の場合のみ探索処理を終了判断し、それ以外の場合は探索処理を継続判断することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート許容値以下でかつ整定時間調整値が整定時間許容値より大きい場合、慣性パラメータを減少させて粘性パラメータ探索部を再実行し、
前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート量許容値より大きくかつ整定時間調整値が整定時間許容値以下の場合、慣性パラメータを増加させて粘性パラメータ探索部を再実行し、前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート量許容値より大きく、かつ、整定時間調整値が整定時間許容値より大きい場合、オーバシュート量許容値あるいは整定時間許容値のいずれか一方をその調整値に再設定して粘性パラメータ探索部を再実行する、ことを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、前記パラメータ初期値算出部は、前記エンドエフェクタと把持ワークの重さ、前記ロボットマニピュレータの位置制御系の帯域を設定することで、パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値を算出することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置とするものである。

また本発明は、ロボットマニピュレータのエンドエフェクタに設けた力センサ情報を基に構成されたインピーダンス制御のパラメータを、力フィードバックの時間応答を繰り返し測定することによって調整する方法であって、
（ｅ）パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値算出ステップと、
（ａ）力フィードバックの時間応答に関するオーバシュート量許容値（上限）と整定時間許容値（上限）を設定する許容値設定ステップと、
（ｂ）慣性パラメータを固定して、整定時間が最小になるような粘性パラメータを探索する粘性パラメータ探索ステップと
（ｃ）前記粘性パラメータ探索ステップの実行結果によって得られるオーバシュート量調整値と整定時間調整値を前記許容値と比較することによって、繰り返し処理を終了するか、継続するかを判断する終了判断ステップと、
（ｄ）前記終了判断ステップが処理を継続すると判断した場合に、慣性パラメータを増減するか、オーバシュート量許容値と整定時間許容値のいずれか一方を緩和した後、粘性パラメータ探索ステップを再実行する慣性パラメータ調整ステップと、
を実行するインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記粘性パラメータ探索ステップ（ｂ）は、
（ｂ１）粘性パラメータＤの初期値、粘性パラメータＤの探索幅ΔＤと探索方向係数α、探索幅の減少率γ、整定時間の変化量に関する閾値δＴを初期設定するステップと、
（ｂ２）インピーダンス制御へステップ状の力指令を入り切りすることで、ハンドエフェクタが把持したワークを対象ワークに押し当てて戻すステップと、
（ｂ３）前記押し当て時に力フィードバックの時間応答を計測して保存するステップと、
（ｂ４）前記力フィードバックの時間応答の評価値として、時間応答の整定時間とオーバシュート量および振動回数を計測・保存するステップと、
（ｂ５）前記振動回数が第１の閾値より大きい場合に前記探索方向係数αを１とし、前記振動回数が第２の閾値より小さい場合に前記探索方向係数αを−１とするステップと、
（ｂ６）前記振動回数が第２の閾値以上でかつ第１の閾値以下であり、整定時間が前回に比べて増加した場合、前記探索方向係数αの符号を反転させるとともに、前記探索幅ΔＤに減少率γをかけて探索幅ΔＤを更新するステップと、
（ｂ７）整定時間の前回からの変化量ΔＴが前記閾値δＴより大きい場合、粘性パラメータＤを、Ｄ＝Ｄ＋α×ΔＤにより更新して（ｂ２）に戻るステップと、
（ｂ８）前記ΔＴが前記δＴ以下の場合は、処理を終了するステップと、
を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記ステップ（ｂ４）において、ステップ状の前記力指令値を中心に上限および下限を指定した整定範囲でもって整定時間を計測することを特徴とする請求項１０に記載のインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記ステップ（ｂ４）において、力フィードバック応答が前記整定範囲を上回った回数と下回った回数を交互に計測し、その回数の和でもって振動回数を計測することを特徴とする請求項１１に記載のインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記終了判断ステップ（ｃ）は、オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値以下の場合のみ探索処理を終了判断し、それ以外の場合は探索処理を継続判断することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記慣性パラメータ調整ステップ（ｄ）は、
（ｄ１）慣性パラメータＭの初期値と探索幅ΔＭ、探索方向係数βおよび探索幅ΔＭの減少率εを初期設定するステップと、
（ｄ２）前記粘性パラメータ探索ステップを実行するステップと、
（ｄ３−１）オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値より大きい場合、前記探索方向係数βを−１にし、
（ｄ３−２）オーバシュート量調整値がオーバシュート量許容値より大きくかつ整定時間調整値が整定時間許容値以下の場合、前記探索方向係数βを１にし、
（ｄ３−３）前記探索係数βの符号が反転した場合は前記探索幅ΔＭに減少率εをかけて値を更新した後、慣性パラメータをＭ＝Ｍ＋β×ΔＭにより更新して（ｄ２）に戻るステップと、
（ｄ３−４）オーバシュート量調整値がオーバシュート量許容値より大きく、かつ、整定時間調整値が整定時間許容値より大きい場合、オーバシュート量許容値あるいは整定時間許容値のいずれか一方をその調整値に再設定して（ｄ２）に戻るステップと、
を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法とするものである。

また本発明は、前記ステップ（ｅ）は、
（ｅ１）エンドエフェクタと把持ワークの重さを設定することで調整時の慣性パラメータの初期値とするステップと、
（ｅ２）前記ロボットマニピュレータの位置制御系の帯域ωｃを設定するステップと、
（ｅ３）剛性パラメータＫを前記慣性パラメータＭと前記ωｃから算出するステップと、
（ｅ４）減衰係数ζを設定するステップと、
（ｅ５）粘性パラメータを前記慣性パラメータと前記剛性パラメータと前記減衰係数から算出し、調整時の粘性パラメータの初期値とするステップと、
を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法とするものである。

本発明によると、エンドエフェクタが把持したワークを対象ワークに繰り返し押し当てる試験指令印加部と、押し当ての度に力センサからの力フィードバックの時間応答を記録してその整定時間とオーバシュート量と振動回数を自動計測する評価基準計測部と、評価基準値あるいは評価基準値の差分値に基づき、インピーダンス制御の慣性パラメータと粘性パラメータを増減するパラメータ探索部を設けたので、個人のスキルに依存せずにインピーダンスパラメータを自動調整できるという効果がある。
また、本発明によると、力応答の振動回数に応じて整定時間が減少する方向に粘性パラメータを増減し、力応答のオーバシュート量に応じてオーバシュート量が減少する方向に慣性パラメータを増減するので、整定時間とオーバシュート量が最小となる最適なインピーダンスパラメータを調整できるという効果がある。

本発明におけるインピーダンス制御パラメータ調整装置の構成図 本発明におけるインピーダンス制御パラメータ調整方法のフローチャート 本発明の粘性パラメータ探索処理のフローチャート 本発明の終了判断処理および慣性パラメータ探索処理のフローチャート 本発明のステップ状力指令および力応答の整定時間、オーバシュート量、振動回数を説明する図 本発明の粘性パラメータの探索過程の模式図 一般的な産業用ロボットの構成図 インピーダンス制御の装置構成図 インピーダンス制御のブロック図 パラメータ初期値算出部のフローチャート

以下、本発明の実施の形態について図を参照して説明する。
まず、一般的な産業用ロボットとインピーダンス制御のシステム構成を説明し、その後、本発明に係わる技術を説明する。

図７は、一般的な産業用ロボットの構成図である。本構成図はマニピュレータとコントローラと可搬式教示操作盤から成る。図７において、１０１はロボットであり、複数の関節軸とリンクを有するマニピュレータである。各関節軸には、エンコーダ付きの駆動モータが内蔵されており、各関節を独立に駆動することができる。１０２はロボット１０１のコントローラであり、各関節軸駆動モータのエンコーダ信号を基にフィードバック制御（位置制御系）を構成し、ロボット１０１の運動を制御するための装置である。１０３は可搬式教示操作盤であり、教示者がロボットを手動（JOG）操作したり、動作プログラムを作成・編集したりするためのインターフェイスである。可搬式教示操作盤１０３は主に操作ボタン群１０３ａと表示画面１０３ｂで構成されている。１０４はロボット１０１の手首部に設けられたエンドエフェクタである。エンドエフェクタはアプリケーションに応じて様々なツールを取り付ける。図９の場合は部品を把持するためのハンド（グリッパ）を取り付けている。

図８はインピーダンス制御された産業用ロボットの構成図を示している。１０５はロボット１０１の手首に取り付けられた６軸力センサである。この６軸力センサは、XYZ各軸方向の力と各軸回りのモーメントを計測できる。１０６はコントローラ１０２内部に構成されたインピーダンス制御部であり、力センサ１０５と各軸エンコーダからの信号を基にフィードバック制御系を構成している。インピーダンス制御部１０６からは各駆動モータのトルク指令または電流指令がアクチュエータ駆動アンプ部１０７へ出力され、そのトルク（または電流）指令値を基にアクチュエータ駆動アンプ部１０７が各駆動モータに電力を供給する。１０８は可搬式教示操作盤で作成（教示）された動作プログラムをコントローラ内部で記憶する動作プログラム記憶部である。１０９は動作プログラム記憶部１０８に記憶された動作プログラムを解釈・実行し、インピーダンス制御部１０６に動作指令を与える動作プログラム実行部である。１１０ａはエンドエフェクタ１０４が把持したワーク（例えば嵌合部品）であり、１１０ｂは対象ワーク（例えば被嵌合部品）である。嵌合作業を例に取れば、ロボット１０１をインピーダンス制御状態にしてそのパラメータを適切に調整することによって、位置姿勢誤差を許容して部品同士を嵌合することが可能になる。

図９にインピーダンス制御の制御ブロック図を示す。図９において、１０６ａは位置制御系であり、各関節軸の位置指令と現在位置（フィードバック）を基に各駆動モータのトルク（または電流指令）をアクチェータ駆動アンプ部１０７に出力する。Frefは力モーメント指令（力モーメント目標値）、Ffbは力モーメントフィードバック値である。θrefは動作プログラム実行部１０９から送られてくる位置指令（関節座標系）、δθはインピーダンス制御演算部１０６ｂが計算する位置修正量である。インピーダンス制御演算部１０６ｂでは、まず、FrefとFfbを基に、次式にしたがって直交座標系における位置修正量δPが計算される（１０６ｃ）。

δP ＝（Ｍｓ^２＋Ｄｓ＋Ｋ）^-1（Fref−Ffb） ・・・（１）
ここで、Ｍ，Ｄ，Ｋはそれぞれ、慣性マトリクス、粘性係数マトリクス、剛性マトリクス（バネ定数）である。通常、これらは対角行列として、各軸方向独立なインピーダンス特性を設定する。また、ｓはラプラス演算子であり、時間に関する一階微分に相当する。
直交座標系における位置修正量δPは、ヤコビ行列Ｊ(θ)を用いて次式により関節座標系の位置修正量δθに分解される（１０６ｄ）。

δθ＝Ｊ(θ)^-1 δP ・・・（２）
このδθをθrefに足し合わせた位置指令θref’を位置制御系１０６ａに与えることによって、外力やモーメントに対して、Ｍ，Ｄ，Ｋで指定された特性を保ちながらロボットが動作する。例えば、剛性マトリクスＫにより外力に対してロボットがバネのように動作し、その際、慣性マトリクスＭおよび粘性係数マトリクスＤを小さくすることによって軽くスムーズに動作する。
本発明は、これら３つのパラメータのうち、慣性Ｍと粘性Ｄの調整技術に関するものである。

図１は本発明に係わるインピーダンス制御装置の構成図である。図１は大別するとロボット１０１とコントローラ１０２から構成される。
図１において、１１１はインピーダンス制御部１０６に対して、ステップ状の力指令を繰り返し入り切りする力指令印加部である。図５（ａ）は力指令印加部１１１が出力する力指令の例（５０１）を示している。一定時間（T1）大きさFstpの力指令を出力し、その後、力指令をゼロに戻す。この力指令を対象ワーク１１０ｂの方向に繰り返し与えることによって、把持ワーク１１０ａを対象ワーク１１０ｂに繰り返し押し当てることができる。
１１２は評価基準計測部である。評価基準計測部は、押し当て動作の度に力センサフィードバックの時間応答（力応答）を記録し、その時間応答からパラメータ調整のための評価基準値として、整定時間、オーバシュート量、振動回数を自動計測する。

図５（ｂ）および（ｃ）は評価基準値の計測原理を示している。整定時間については、まず、ステップ状の力指令値５０１に対してその上限値５０２と下限値５０３を定めて整定範囲５０４を設ける。力応答５０５が整定範囲５０４内に収束し始める（整定範囲内に入ってそれ以降出てこなくなる）時間５０６を整定時間とする。オーバシュート量については、力応答５０５がステップ状の力指令値を超えて最大となった値５０７をオーバシュート量とする。振動回数については、力応答が整定範囲５０４を外れた回数をカウントする。ただし、上限値５０２を上回った場合、下限値５０３を上回った場合が交互に発生したときのみカウントする。図５（ｃ）の力応答５０８ａの場合、上限値５０２を一度も上回っていないので、振動回数はゼロとする。力応答５０８ｂの場合、上限値５０２を一度上回った後は整定範囲５０４内に収束して下限値５０３を下回っていないので、振動回数は１回とカウントする。力応答５０８ｃの場合、上限値５０２を上回った後、整定範囲５０４を通り越して下限値５０３を下回ってから整定しているので、振動回数は２回とカウントする。

図１において、１１３は整定時間とオーバシュート量に対する許容値を設定する許容値設定部である。１１４は、慣性パラメータＭを固定した状態で、繰り返し押し当てをする過程で整定時間が最小となる粘性パラメータＤを探索する粘性パラメータ探索部である。探索方法の詳細については後述する。１１５は、粘性パラメータ探索の結果で得られた整定時間（整定時間調整値）とオーバシュート量（オーバシュート量調整値）を許容値設定部１１３で設定した整定時間許容値およびオーバシュート量許容値と比較し、調整処理を終了するか継続するかを判断する終了判断部である。終了判断の方法については後述する。１１６は、終了判断部１１５がパラメータ調整処理を継続すると判断した場合に、慣性パラメータＭの値を変更する慣性パラメータ調整部である。慣性パラメータ調整部１１６が慣性パラメータを変更した後、粘性パラメータ探索部１１４が再実行される。終了判断部１１５が終了判断するまで、粘性パラメータ探索部と慣性パラメータ調整部が繰り返し実行される。
１１７はパラメータ初期値算出部である。パラメータ初期値算出部は、設定されたエンドエフェクタと把持ワークの重さ、ロボットマニピュレータの位置制御系の帯域からパラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値が算出される。

以上説明したように本発明によれば、パラメータ初期値算出部でエンドエフェクタと把持ワークの重さ、ロボットマニピュレータの位置制御系の帯域を設定し、許容値設定部で整定時間とオーバシュート量の許容値を設定するだけで、粘性パラメータ探索部と終了判断部と慣性パラメータ調整部が、粘性パラメータと慣性パラメータの調整を明確な評価基準の基に繰り返し実行するので、個人のスキルに依存することなく、特別な知識がなくても複数のパラメータを調整できるという効果がある。

図２は、本発明に係わるインピーダンス制御パラメータ調整方法のフローチャートである。
図２において、S201では、調整（押し当て動作）する座標軸方向について、インピーダンス制御の慣性パラメータＭと粘性パラメータＤの初期値を設定する。S202では、力応答の整定時間とオーバシュート量の許容値を設定する。S203では、慣性パラメータＭを固定した状態で、繰り返し押し当て動作を実行することで、力応答の整定時間が最小となる粘性パラメータを探索する。S204では、粘性パラメータ探索の結果得られた整定時間（整定時間調整値）とオーバシュート量（オーバシュート量調整値）を比較して処理を継続するか、終了するかを判断する。S204で処理を継続すると判断された場合、S205にて慣性パラメータが調整（微調整）され、S203に戻り、粘性パラメータが再探索される。S203からS205までの一連の処理はS204にて終了判断されるまで繰り返し実行される。
つぎに、S203の粘性パラメータ探索方法の詳細について説明する。

図３は、粘性パラメータ探索S203の詳細フローチャートを示している。
図３において、S301では、力指令値５０１の大きさFstpおよび印加時間T1,T2、粘性パラメータＤの探索幅ΔＤ、探索方向係数α（１または−１）、探索幅の減少率γ（１未満）、整定時間の減少幅閾値δＴの初期値を設定する。S302では、図５（ａ）に示したようにステップ状の力指令５０１をインピーダンス制御部１０６に印加する。S303では、力センサフィードバックの時間応答（力応答）を記録（計測・保存）する。S304では、図５（ｂ）（ｃ）に示したように、記録した力応答について、整定時間、オーバシュート量および振動回数を自動計測する。S305では、自動計測した振動回数が予め設定した閾値１（例えば２）を超えているかどうか判断し、超えている場合は、探索方向係数αを１（増加）に設定する（S306）。振動回数が閾値１以下の場合は、S307で振動回数が予め設定した閾値２（例えば１）を下回っているかどうか判断し、下回っている場合は、探索方向係数αを−1（減少）に設定する（S308）。振動回数が多い（閾値１を超える）ということは、押し当て時の制御系が減衰不足になっていると考えられるので、次回の押し当て時に粘性パラメータＤを増加させることを意味する。振動回数が少ない（閾値２を下回る）ということは、押し当て時の制御系が過減衰になっていると考えられるので、次回の押し当て時に粘性パラメータを減少させることを意味する。

振動回数が閾値２以上で閾値１以下の場合は、S309にて整定時間が前回の押し当て時よりも増加したかどうか判断し、整定時間が増加した場合、探索方向係数αの符号を反転させ、さらに探索幅ΔＤに減衰率γをかけた値を新たな探索幅ΔＤとして再設定する（S310）。
つぎにS311にて前回押し当て時からの整定時間の減少幅が閾値δＴ以下であるかどうか判断し、δＴ以下の場合は、整定時間が十分減少したと考え、処理を終了する（S312）。整定時間の減少幅が閾値δＴを超える場合は、S313にて、次式に基づいて粘性パラメータＤを更新する。

Ｄ（更新値）＝Ｄ（現状の値）＋α×ΔＤ ・・・（３）
粘性パラメータＤを更新した後、S302に戻って力指令を再び印加して、以下、上述した処理を繰り返す。

図６は上述した粘性パラメータ探索が進行する様子を模式的に示したものである。図６において、横軸は粘性パラメータ、縦軸はその粘性パラメータで押し当てを実施したときの整定時間の関係を示している。図に示したように、整定時間が最小となるような最適な粘性パラメータが存在し、極小値（ローカルミニマム）は存在しないと考えられる。601から606は探索した順番を示している。601は1回目の押し当て時の粘性パラメータと計測した整定時間であり、602は2回目、603は3回目（以下、同様）を示している。1回目の突き当て時は振動回数が、閾値２未満で過減衰であったため、探索方向係数αは−1に設定され、2回目、3回目の押し当てが実行される。その際、初期設定した探索幅ΔＤだけ、粘性パラメータＤを減少させて押し当てを実行する。3回目までは整定時間が減少するが、4回目で整定時間が増加に転じている。整定時間が増加に転じた場合は、探索方向係数αの符号を反転させ、探索幅ΔＤに減少率（例えば0.3）をかける。そのため、5回目以降は、図６に示したように、Ｄを増加させる方向（α＝１）に4回目までより細かい探索幅で探索が進行する。図から明らかなように、このような処理を繰り返すことによって、しだいに整定時間が最小となる状態に探索が収束していく。整定時間の減少幅が閾値δＴ以下になったときに探索を終了する（閾値δＴの設定によっては6回目の探索で終了する）。
以上説明した探索処理は、減衰不足（振動回数＞閾値１）場合であっても同様に機能して整定時間が最小となる粘性パラメータを探索可能である。

以上、図３に基づいて粘性パラメータ探索処理の詳細を説明したが、つぎに、図２における終了判断S204と慣性パラメータ調整S205の詳細を説明する。

図４は、終了判断S204と慣性パラメータ調整S205の詳細フローチャートを示している。
図４において、点線が囲んだ範囲がS204とS205に該当している。まず、終了判断S204の詳細から説明する。S401では、粘性パラメータ探索S203（図３）の結果得られたオーバシュート量の調整値がS202で設定したオーバシュート量許容値以下であるかどうか判断する。調整値が許容値以下である場合は、S402おいて、整定時間調整値が整定時間許容値以下であるかどうか判断する。オーバシュート量も整定時間も許容値以下の場合は、適切な調整がなされたとして終了する（S206）。オーバシュート量調整値は許容値以下であるが、整定時間調整値が許容値を超える場合は、慣性パラメータＭが重過ぎるので、慣性パラメータの探索方向係数βを−１に設定する（S404）。

一方、S401でオーバシュート量調整値が許容値を超えると判断された場合も同様に整定時間調整値が許容値以下であるかどうかが判断される（S403）。オーバシュート量調整値が許容値を超え、整定時間調整値が許容値以下の場合は、慣性パラメータＭが軽すぎるので、慣性パラメータの探索方向係数βを１に設定する（S405）。つぎにS406では、探索方向係数βの符号（正負）が変化したかどうか判断し、符号が変化（反転）した場合は、慣性パラメータの探索幅ΔＭに減少率ε（１未満）をかけた値を新たな探索幅ΔＭとして再設定する（S407）。つぎにS408にて次式に基づいて慣性パラメータを更新し、粘性パラメータ探索S203に戻る。

Ｍ（更新値）＝Ｍ（現在値）＋β×ΔＭ ・・・（４）
一方、オーバシュート量調整値も整定時間調整値も許容値を超える場合は、許容値の設定が厳しすぎるので、S409にて許容値を緩和する方向に再設定する。

図１０は、パラメータ初期値算出部のフローチャートである。パラメータ初期値算出部で決定した慣性パラメータＭと粘性パラメータＤの初期値は、インピーダンス制御パラメータ調整方法の中の初期値設定S201で設定される。
S702では、エンドエフェクタと把持ワークの重さの和Ｍ０を設定する。S703では、ロボットの位置制御系の帯域ωｃを設定する。S704では、減衰係数ζを設定する。S705では、S702で設定したエンドエフェクタと把持ワークの重さの和Ｍ０が調整時の慣性パラメータＭの初期値となる。S704では、S705で決定した慣性パラメータＭとS703で設定したロボットの位置制御系の帯域ωｃと次式から剛性パラメータＫが算出される。

Ｋ＝Ｍ(ωｃ)^２ ・・・（５）
S707では、S703で決定した慣性パラメータＭ、S704で決定した剛性パラメータＫ、S704で設定した減衰係数ζと次式から調整時の粘性パラメータＤの初期値が算出される。

Ｄ＝ζ×２√（ＭＫ） ・・・（６）

以上説明したように本発明によれば、調整時の慣性パラメータと粘性パラメータの初期値が自動で設定され、押し当て時の力応答の振動回数と整定時間の増減に応じて粘性パラメータの探索方向と探索幅を自動調整し、オーバシュート量と整定時間の調整値に応じて、慣性パラメータの探索方向と探索幅を自動調整するので、短時間で恒に最適なパラメータを調整できるという効果がある。

本発明のインピーダンス制御調整装置および調整方法によって、対象作業（組立作業やバリ取り作業など）や対象ワーク（材質）およびロボット（エンドエフェクタ含む）に応じた最適なインピーダンス制御パラメータを調整できる。

１０１ ロボット
１０２ コントローラ
１０３ 可搬式教示操作盤
１０３ａ 操作ボタン群
１０３ｂ 表示画面
１０４ エンドエフェクタ
１０５ 力センサ
１０６ インピーダンス制御部
１０６ａ 位置制御系
１０６ｂ インピーダンス制御演算部
１０６ｃ インピーダンスモデル
１０６ｄ 速度分解演算部
１０７ アクチュエータ駆動アンプ部
１０８ 動作プログラム記憶部
１０９ 動作プログラム実行部
１１０ａ 把持ワーク
１１０ｂ 対象ワーク
１１１ 力指令印加部
１１２ 評価基準計測部
１１３ 許容値設定部
１１４ 粘性パラメータ探索部
１１５ 終了判断部
１１６ 慣性パラメータ調整部
１１７ パラメータ初期値算出部
５０１ ステップ状力指令値
５０２ 整定判断のための上限値
５０３ 整定判断のための下限値
５０４ 力応答の整定範囲
５０５ 力応答の例
５０６ 整定時間
５０７ オーバシュート量
５０８ａ 力応答の例（振動回数０回）
５０８ｂ 力応答の例（振動回数１回）
５０８ｃ 力応答の例（振動回数２回）
６０１ 押し当て１回目での粘性パラメータと整定時間
６０２ 押し当て２回目での粘性パラメータと整定時間
６０３ 押し当て３回目での粘性パラメータと整定時間
６０４ 押し当て４回目での粘性パラメータと整定時間
６０５ 押し当て５回目での粘性パラメータと整定時間
６０６ 押し当て６回目での粘性パラメータと整定時間

Claims (15)

1. ロボットマニピュレータのエンドエフェクタに設けた力センサ情報を基に構成されたインピーダンス制御のパラメータを調整する装置であって、
   パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値を算出するパラメータの初期値算出部と、
   インピーダンス制御へステップ状の力指令を入り切りすることで、ハンドエフェクタが把持したワークを対象ワークに繰り返し押し当てる力指令印加部と、
   押し当ての度に力センサからの力フィードバックの時間応答を記録するとともに、力フィードバックの時間応答の整定時間とオーバシュート量と振動回数を自動計測する評価基準計測部と、
   許容できるオーバシュート量の最大値であるオーバシュート量許容値と、許容できる整定時間の最大値である整定時間許容値を設定する許容値設定部と、
   慣性パラメータを固定し、繰り返し押し当てを実行して整定時間が最小になるような粘性パラメータを探索する粘性パラメータ探索部と、
   粘性パラメータ探索部による粘性パラメータ探索の結果、評価値計測部から得られる整定時間調整値とオーバシュート量調整値を、前記許容値と比較することによって繰り返し処理を終了するか、継続するかを判断する終了判断部と、
   終了判断部が処理を継続すると判断した場合に、慣性パラメータを増減するか、オーバシュート量許容値と整定時間許容値のいずれか一方を緩和した後、粘性パラメータ探索部を再実行する慣性パラメータ調整部と、
   を備えたインピーダンス制御のパラメータ調整装置。
2. 前記評価基準計測部は、ステップ状の前記力指令値を中心に上限および下限を指定した整定範囲でもって整定時間を計測することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置。
3. （振動回数の計測方法）
   前記評価基準計測部は、力フィードバック応答が前記整定範囲を上回った回数と下回った回数を交互に計測し、その回数の和でもって振動回数を計測することを特徴とする請求項２に記載のインピーダンス制御のパラメータ調整装置。
4. 前記粘性パラメータ探索部は、前記振動回数が予め設定した第１の閾値を上回る場合に粘性パラメータを増加させ、予め設定した第２の閾値を下回る場合に粘性パラメータを減少させることを特徴とする請求項３に記載のインピーダンス制御のパラメータ調整装置。
5. 前記粘性パラメータ探索部は、前記振動回数が前記第２の閾値以上で前記第１の閾値以下であるとき、整定時間が前回に比べて増加した場合に、粘性パラメータの探索方向を反転させるとともに、探索幅を予め設定した比率で減少させ、整定時間の前回からの変化量が予め設定した閾値以下になったときに探索処理を終了することを特徴とする請求項３または４に記載のインピーダンス制御のパラメータ調整装置。
6. 前記終了判断部は、オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値以下の場合のみ探索処理を終了判断し、それ以外の場合は探索処理を継続判断することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置。
7. 前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート許容値以下でかつ整定時間調整値が整定時間許容値より大きい場合、慣性パラメータを減少させて粘性パラメータ探索部を再実行し、
   前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート量許容値より大きくかつ整定時間調整値が整定時間許容値以下の場合、慣性パラメータを増加させて粘性パラメータ探索部を再実行し、
   前記慣性パラメータ調整部は、オーバシュート量調整値がオーバシュート量許容値より大きく、かつ、整定時間調整値が整定時間許容値より大きい場合、オーバシュート量許容値あるいは整定時間許容値のいずれか一方をその調整値に再設定して粘性パラメータ探索部を再実行する、
   ことを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置。
8. 前記パラメータ初期値算出部は、前記エンドエフェクタと把持ワークの重さ、前記ロボットマニピュレータの位置制御系の帯域を設定することで、パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値を算出することを特徴とする請求項１に記載のインピーダンス制御のパラメータ調整装置。
9. ロボットマニピュレータのエンドエフェクタに設けた力センサ情報を基に構成されたインピーダンス制御のパラメータを、力フィードバックの時間応答を繰り返し測定することによって調整する方法であって、
   （ｅ）パラメータ調整時に設定する慣性パラメータと粘性パラメータの初期値算出ステップと、
   （ａ）力フィードバックの時間応答に関するオーバシュート量許容値（上限）と整定時間許容値（上限）を設定する許容値設定ステップと、
   （ｂ）慣性パラメータを固定して、整定時間が最小になるような粘性パラメータを探索する粘性パラメータ探索ステップと
   （ｃ）前記粘性パラメータ探索ステップの実行結果によって得られるオーバシュート量調整値と整定時間調整値を前記許容値と比較することによって、繰り返し処理を終了するか、継続するかを判断する終了判断ステップと、
   （ｄ）前記終了判断ステップが処理を継続すると判断した場合に、慣性パラメータを増減するか、オーバシュート量許容値と整定時間許容値のいずれか一方を緩和した後、粘性パラメータ探索ステップを再実行する慣性パラメータ調整ステップと、
   を実行するインピーダンス制御のパラメータ調整方法。
10. 前記粘性パラメータ探索ステップ（ｂ）は、
    （ｂ１）粘性パラメータＤの初期値、粘性パラメータＤの探索幅ΔＤと探索方向係数α、探索幅の減少率γ、整定時間の変化量に関する閾値δＴを初期設定するステップと、
    （ｂ２）インピーダンス制御へステップ状の力指令を入り切りすることで、ハンドエフェクタが把持したワークを対象ワークに押し当てて戻すステップと、
    （ｂ３）前記押し当て時に力フィードバックの時間応答を計測して保存するステップと、
    （ｂ４）前記力フィードバックの時間応答の評価値として、時間応答の整定時間とオーバシュート量および振動回数を計測・保存するステップと、
    （ｂ５）前記振動回数が第１の閾値より大きい場合に前記探索方向係数αを１とし、前記振動回数が第２の閾値より小さい場合に前記探索方向係数αを−１とするステップと、
    （ｂ６）前記振動回数が第２の閾値以上でかつ第１の閾値以下であり、整定時間が前回に比べて増加した場合、前記探索方向係数αの符号を反転させるとともに、前記探索幅ΔＤに減少率γをかけて探索幅ΔＤを更新するステップと、
    （ｂ７）整定時間の前回からの変化量ΔＴが前記閾値δＴより大きい場合、粘性パラメータＤを、Ｄ＝Ｄ＋α×ΔＤにより更新して（ｂ２）に戻るステップと、
    （ｂ８）前記ΔＴが前記δＴ以下の場合は、処理を終了するステップと、
    を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法。
11. 前記ステップ（ｂ４）において、ステップ状の前記力指令値を中心に上限および下限を指定した整定範囲でもって整定時間を計測することを特徴とする請求項１０に記載のインピーダンス制御のパラメータ調整方法。
12. 前記ステップ（ｂ４）において、力フィードバック応答が前記整定範囲を上回った回数と下回った回数を交互に計測し、その回数の和でもって振動回数を計測することを特徴とする請求項１１に記載のインピーダンス制御のパラメータ調整方法。
13. 前記終了判断ステップ（ｃ）は、オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値以下の場合のみ探索処理を終了判断し、それ以外の場合は探索処理を継続判断することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法。
14. 前記慣性パラメータ調整ステップ（ｄ）は、
    （ｄ１）慣性パラメータＭの初期値と探索幅ΔＭ、探索方向係数βおよび探索幅ΔＭの減少率εを初期設定するステップと、
    （ｄ２）前記粘性パラメータ探索ステップを実行するステップと、
    （ｄ３−１）オーバシュート量調整値がオーバシュート量許容値以下でかつ整定時間調整値が整定時間許容値より大きい場合、前記探索方向係数βを−１にし、
    （ｄ３−２）オーバシュート量調整値がオーバシュート量許容値より大きくかつ整定時間調整値が整定時間許容値以下の場合、前記探索方向係数βを１にし、
    （ｄ３−３）前記探索係数βの符号が反転した場合は前記探索幅ΔＭに減少率εをかけて値を更新した後、慣性パラメータをＭ＝Ｍ＋β×ΔＭにより更新して（ｄ２）に戻るステップと、
    （ｄ３−４）オーバシュート量調整値がオーバシュート量許容値より大きく、かつ、整定時間調整値が整定時間許容値より大きい場合、オーバシュート量許容値あるいは整定時間許容値のいずれか一方をその調整値に再設定して（ｄ２）に戻るステップと、
    を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法。
15. 前記ステップ（ｅ）は、
    （ｅ１）エンドエフェクタと把持ワークの重さを設定することで調整時の慣性パラメータの初期値とするステップと、
    （ｅ２）前記ロボットマニピュレータの位置制御系の帯域ωｃを設定するステップと、
    （ｅ３）剛性パラメータＫを前記慣性パラメータＭと前記ωｃから算出するステップと、
    （ｅ４）減衰係数ζを設定するステップと、
    （ｅ５）粘性パラメータを前記慣性パラメータと前記剛性パラメータと前記減衰係数から算出し、調整時の粘性パラメータの初期値とするステップと、
    を実行することを特徴とする請求項９に記載のインピーダンス制御のパラメータ調整方法。

Images