力制御装置
【課題】力制限を超過した際にロボットを停止処理する場合においても、作用力が許容値以下となる制約を満たす範囲で、動作時間の短縮化、立上げ調整時間の短縮化を図る。
【解決手段】対象物に対するロボットの目標位置指令を生成する指令生成手段（１０）と、目標位置指令に応じて追従制御を行うロボット制御手段（２０）と、対象物からロボットへ働く作用力が所定の力制限値を超過した場合に停止指令を生成する力制限超過判別手段（３０）と、ロボットを減速停止させる場合を考慮して、対象物の特性および所定の力制限値に基づいて、減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で動作速度を最大にする最適速度を算出する速度最適化手段（４０）とを備え、指令生成手段（１０）は、最適速度に応じた目標位置指令を生成し、停止指令を受信した場合には、ロボットを減速停止させる指令を生成する。
【解決手段】対象物に対するロボットの目標位置指令を生成する指令生成手段（１０）と、目標位置指令に応じて追従制御を行うロボット制御手段（２０）と、対象物からロボットへ働く作用力が所定の力制限値を超過した場合に停止指令を生成する力制限超過判別手段（３０）と、ロボットを減速停止させる場合を考慮して、対象物の特性および所定の力制限値に基づいて、減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で動作速度を最大にする最適速度を算出する速度最適化手段（４０）とを備え、指令生成手段（１０）は、最適速度に応じた目標位置指令を生成し、停止指令を受信した場合には、ロボットを減速停止させる指令を生成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、組立、ばりとり、研磨、基板実装、検査などを行うロボット、自動組立装置、実装機、加工機、検査装置などに適用される力制御装置に関する。
【背景技術】
【０００２】
一般的な力制御装置において、部品を収納したマガジンから部品を取り出し、所定位置まで移動して部品組み付け作業を行うロボットの先端部には、力センサが設けられている。ロボットコントローラは、力センサで検出した力のフィードバックによるコンプライアンス制御を行い、検出した力とあらかじめ設定された力の閾値とを比較して、その比較結果により、次のロボット動作の決定、作業成否の判定を行う。
【０００３】
例えば、丸棒状の部品をハンドで把持し、下降し、挿入するといった一連の動作を行う場合、挿入中（下降中）に検出した力が閾値を超えた場合には、ロボットコントローラは、挿入作業が失敗したと判断し、下降動作を停止する。この結果、過大な力の発生を防止することができる。そして、ハンド停止後は、再び作業を行うか、あるいは、異常警報を発生することとなる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
挿入作業の失敗を検知して停止する場合、検知した地点から実際に停止するまでの間に、ロボットは下降する。このため、ロボットおよび対象物には、この下降の間に、検出に用いた閾値以上の力が作用することも考えられる。挿入失敗を検知して停止した際に作用する力（以下の説明においては、このような「作用する力」のことを単に作用力と表現する場合もある）が、ロボットおよび対象物の許容値以下となるようにするには、停止時の作用力が許容値以下となることを保証できる速度以下で下降させる必要がある。
【０００６】
しかしながら、従来の力制御装置は、停止時の作用力が許容値以下となる速度を算出する機能を備えていなかった。このため、必要以上に下降速度を下げる、あるいは、停止時の作用力が許容値以下となる速度を試行錯誤であらかじめ求めておくといった対策が必要であった。しかしながら、前者の場合は、速度が遅いため、動作時間が長くなるといった問題があった。また、後者の場合には、事前調整に時間を要するといった問題があった。
【０００７】
本発明は、前記のような課題を解決するためになされたものであり、挿入失敗等により力制限を超過した際にロボットを動作途中で減速停止する場合でも、作用力が許容値以下となる制約を満たす範囲で動作時間の短縮を図ること、および作用力が許容値以下となる制約を満たす範囲で動作時間の短縮を実現するための調整時間の短縮を図ることのできる力制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る力制御装置は、対象物に対するロボットの目標位置指令を生成する指令生成手段と、目標位置指令に応じてロボットの追従制御を行うロボット制御手段と、追従動作に伴って対象物からロボットへ働く作用力が、対象物もしくはロボットにより決まる作用力の上限値に相当する所定の許容値よりも小さい値として規定される所定の力制限値を超過した場合に、指令生成手段で生成される目標位置指令によりロボットを減速停止させるための停止指令を生成する力制限超過判別手段と、力制限超過判別手段により停止指令が生成されることでロボットを減速停止させる場合を考慮して、対象物の特性および所定の力制限値に基づいて、減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度を算出する速度最適化手段とを備え、指令生成手段は、速度最適化手段で算出された最適速度に応じた目標位置指令を生成するとともに、力制限超過判別手段で生成された停止指令を受信した場合には、ロボットを減速停止させる指令を生成するものである。
【発明の効果】
【０００９】
本発明に係る力制御装置によれば、対象物からロボットへ働く作用力が所定の力制限を超過したことでロボットを減速停止させる必要が生じた際に、減速停止中の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成してロボットを動作させることで、作用力が許容値以下となる制約を満たす範囲でロボット動作時間を短縮でき、ロボット動作時間短縮のための事前の立上げ調整時間の短縮化を図ることのできる力制御装置を得ることができる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施の形態１における力制御装置の構成を示すブロック図である。
【図２】本発明の実施の形態１におけるロボット制御手段の内部構成図である。
【図３】本発明の実施の形態１における各軸位置制御手段の構成図の一例である。
【図４】本発明の実施の形態１における力覚考慮補正手段の内部構成図である。
【図５】本発明の実施の形態２における力制御装置の構成を示すブロック図である。
【図６】本発明の実施の形態３における力制御装置の構成を示すブロック図である。
【図７】本発明の実施の形態４における力制御装置の構成を示すブロック図である。
【図８】本発明の実施の形態５における力制御装置の構成を示すブロック図である。
【発明を実施するための形態】
【００１１】
以下、本発明の力制御装置の好適な実施の形態につき図面を用いて説明する。
【００１２】
実施の形態１．
図１は、本発明の実施の形態１における力制御装置の構成を示すブロック図である。本実施の形態１における力制御装置は、指令生成手段１０、ロボット制御手段２０、力制限超過判別手段３０、および速度最適化手段４０を備えており、制御対象であるロボット１の追従制御を行っている。このロボット１は、１軸もしくは複数軸のモータを備えるものであり、その一例として、産業用ロボットと呼ばれる６軸垂直多関節ロボット、あるいは４軸水平多関節ロボットが挙げられる。
【００１３】
指令生成手段１０は、ロボット１に時々刻々の目標位置を与える。ロボット制御手段２０は、指令生成手段１０から与えられた位置指令に応じて、ロボット１を追従制御する。
【００１４】
ロボット１の先端部近辺もしくはロボット１が作業する対象には、作用する力とモーメントを計測するための力覚センサ２が取り付けられている。そして、力覚センサ２で測定された力およびモーメントは、力検出値としてロボット制御手段２０に入力されるとともに、力制限超過判別手段３０にも入力される。
【００１５】
力検出値は、６軸の力覚センサ２を用いている場合には、３軸方向の力と各軸周りのモーメントで構成され、５軸以下の力覚センサ２を用いている場合には、３軸方向の力と各軸周りのモーメントのうち、計測できるデータのみで構成される。
【００１６】
また、ロボット１を駆動する各軸のモータには、エンコーダやレゾルバなどの各軸のモータ位置を測定する位置センサ３がそれぞれ取り付けられており、こうした位置センサ３で測定した各軸のモータ位置は、ロボット制御手段２０に入力される。
【００１７】
力制限超過判別手段３０は、軸ごとにあらかじめ記憶された所定の力閾値と力覚センサ２から送信される力検出値とを比較し、少なくとも１つの軸方向の力もしくはモーメントが力閾値を超えた場合には、指令生成手段１０に停止指令を送信する。なお、本実施の形態１では、力制限超過の判別を、検出した力とモーメントの各成分に対してそれぞれ所定の力閾値と比較することで実施しているが、合成した力やモーメントに対して実施してもよい。
【００１８】
速度最適化手段４０は、力制限超過判別手段３０から入力される力制限値（力閾値）、ロボット制御手段２０から入力されるコンプライアンス制御パラメータ、および速度最適化手段４０の内部にあらかじめ設定されている力許容値、対象物の剛性に関するパラメータ（対象物のばね定数）に基づいて、最適速度を求め、指令生成手段１０に出力する。
【００１９】
ここで、力許容値は、力制限値よりも大きい値であり、対象物の制約もしくはロボットの制約の低い方により決まる絶対超えてはいけない作用力の上限値であり、この値を超過することがないようにロボットを制御する値に相当する。一方、力制限値は、力許容値以下となる制約を満たすために設ける制限値で、力制限値を超えたらロボットを減速停止させることで、作用力が力許容値を超えることを防止しようとするものである。
【００２０】
また、速度最適化手段４０が出力する「最適速度」とは、力制限超過判別手段３０で力制限超過を判別し停止する場合に、対象物およびロボット１に作用する力およびモーメントが、速度最適化手段４０の内部に設定されている力許容値以下となる上限速度を意味している。
【００２１】
指令生成手段１０は、速度最適化手段４０から入力された最適速度を最高速度とし、指令生成手段１０の内部に記憶されている加減速パラメータに基づいて、指令生成周期ごとに位置指令を生成し、ロボット制御手段２０に出力する。また、指令生成手段１０は、力制限超過判別手段３０から停止指令を受け取ると、現在の位置・速度から減速停止する指令を生成して、ロボット制御手段２０に送信する。
【００２２】
次に、ロボット制御手段２０の機能について、詳細に説明する。図２は、本発明の実施の形態１におけるロボット制御手段２０の内部構成図である。図２におけるロボット制御手段２０は、力覚考慮補正手段２１、逆変換手段２２、および各軸位置制御手段２３で構成されている。
【００２３】
力覚考慮補正手段２１は、指令生成手段１０で生成された位置指令に基づいて、位置指令修正量を生成する。この位置指令修正量の生成の詳細に関しては、後述する。そして、位置指令は、力覚考慮補正手段２１の出力である位置指令修正量を加算することで補正される。
【００２４】
次に、逆変換手段２２は、位置指令と位置指令修正量との加算値に対して、手先位置姿勢を実現する各軸のモータ位置を算出する逆変換計算を行い、ロボットの各軸の位置指令である関節位置指令に変換する。
【００２５】
変換した関節位置指令は、各軸位置制御手段２３に入力される。各軸位置制御手段２３は、入力された関節位置指令に各軸のモータ位置が追従するように制御を行う。図３は、本発明の実施の形態１における各軸位置制御手段２３の構成図の一例である。
【００２６】
図３に示す各軸位置制御手段２３の内部は、それぞれの軸（１軸〜ｎ軸）が、比例演算部２３ａ、微分演算部２３ｂ、および比例積分演算部２３ｃで構成されている。符号の後ろの（）内の数字は、軸の番号に対応している。
【００２７】
比例演算部２３ａは、各軸の位置指令と当該軸のモータ位置との差に対して比例演算を行う。一方、微分演算部２３ｂは、モータ位置を微分することでモータ速度を算出する。そして、比例積分演算部２３ｃは、比例演算部２３ａの出力から微分演算部２３ｂの出力を減算した値に対して比例積分演算を行う。
【００２８】
上記制御系は、位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。なお、図３に示した一例では、各軸位置制御手段２３の構成として、各軸ごとにモータ位置制御と速度制御を行うフィードバック制御系の構成とした。しかしながら、本発明の制御系は、このような構成に限定されるものではない。各軸ごとにオブザーバを含む制御系であってもよいし、フィードフォワードを含む制御系であっても構わない。また、全て各軸独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であっても構わない。
【００２９】
次に、力覚考慮補正手段２１の内部構成について、詳細に説明する。図４は、本発明の実施の形態１における力覚考慮補正手段２１の内部構成図である。図４に示すように、本実施の形態１における力覚考慮補正手段２１は、ツール座標系座標変換部２１１、スティフネス行列部２１２、微分演算部２１３、ダンピング行列部２１４、比例ゲイン演算部２１５、直交座標系座標変換部２１６、積分演算部２１７、ツール座標系座標変換部２１８、および順変換部２１９を備えている。
【００３０】
まず、順変換部２１９は、各軸モータ位置からそのときの手先位置姿勢を算出する順変換を行い、手先位置姿勢を算出する。ツール座標系座標変換部２１１は、指令生成手段１０で生成された位置指令（手先位置姿勢の指令）と、順変換部２１９で算出された手先位置姿勢との差を入力として、ツール座標系への座標変換を行う。ここで、「ツール座標系」とは、ロボット先端に取り付けたツールやハンドに固定された座標系を意味する。
【００３１】
次に、スティフネス行列部２１２は、ツール座標系座標変換部２１１の出力から構成されるベクトルを入力し、スティフネス行列とのベクトル積を求め、その積を出力する。
【００３２】
一方、微分演算部２１３は、ツール座標系座標変換部２１１の出力の各要素の微分を算出する。そして、ダンピング行列部２１４は、微分演算部２１３で算出された微分結果から構成されるベクトルと、ダンピング行列とのベクトル積を求め、その積を出力する。さらに、スティフネス行列部２１２の出力と、ダンピング行列部２１４の出力とが加算される。
【００３３】
次に、ツール座標系座標変換部２１８は、力覚センサ２で検出された力検出値をツール座標系に変換する。そして、ツール座標系座標変換部２１８により変換された力検出値は、先ほどのスティフネス行列部２１２の出力とダンピング行列部２１４の出力との加算結果から減算され、比例ゲイン演算部２１５に入力される。
【００３４】
比例ゲイン演算部２１５は、入力した減算結果の各要素に対して比例ゲインを乗じる。さらに、直交座標系座標変換部２１６は、比例ゲインの乗算結果を直交座標系に座標変換する。さらに、積分演算部２１７は、直交座標系座標変換部２１６による変換結果に対して積分処理を行い、位置指令修正量（位置指令補正量）を算出し、出力する。このような一連処理により、力覚考慮補正手段２１は、指令生成手段１０で生成された位置指令に基づいて、位置指令修正量を生成することができる。
【００３５】
次に、速度最適化手段４０の内部構成について、詳細に説明する。なお、本実施の形態１では、力制限値（力閾値）と力許容値とを各軸毎に設定する場合を例に挙げて説明するが、各軸方向の力およびモーメントの合成で考えても構わない。また、以下では、代表としてＺ軸方向について説明するが、Ｘ軸方向、Ｙ軸方向の力を考える場合も、同様である。
【００３６】
Ｚ軸方向の力閾値（力制限値）をＦｓ、Ｚ軸方向の力許容値をＦｐ、Ｚ方向の対象物の剛性をＫ１、Ｚ方向のコンプライアンス制御パラメータをＫ２とする。ここで、Ｆｓは、力制限超過判別手段３０から入力される力閾値であり、作用する力がＦｓを超えると、力制限超過判別手段３０から指令生成手段１０に対して、停止指令が入力され、ロボット１は、減速停止する。
【００３７】
また、対象物の剛性Ｋ１および力許容値Ｆｐは、あらかじめ速度最適化手段４０の内部に記憶されている。速度最適化手段４０は、それぞれの軸の動作の直前に、力制限超過判別手段３０から入力される力閾値Ｆｓ、ロボット制御手段２０から入力されるコンプライアンス制御パラメータＫ２（スティフネス行列のＺ軸に対応する対角要素）、あらかじめ記憶されている対象物の許容力Ｆｐ、対象物の剛性Ｋ１、および減速時間ｇｔから、下式（１）により、最適速度Ｖｓを算出する。
Ｖｓ＝ｓｑｒｔ（（Ｆｐ−Ｆｓ）×（Ｋ１＋Ｋ２）×２×ｇｔ／Ｋ１／Ｋ２） （１）
【００３８】
すなわち、速度最適化手段４０は、上式（１）を用いて、対象物の特性および所定の力制限値に基づいて、前記減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度Ｖｓを算出する。
【００３９】
そして、速度最適化手段４０は、算出した最適速度Ｖｓを指令生成手段１０に出力する。なお、上式（１）において、ｓｑｒｔは、平方根を意味している。指令生成手段１０は、最高速度がＶｓとなるように、指令生成周期ごとに位置指令を生成し、ロボット制御手段２０に出力する。
【００４０】
このような制御を行うことで、位置ずれなどにより正常な場合と比べて過大な力がロボットに作用した際に（エラー発生時に相当）、ロボットを減速停止させた場合においても、減速停止中にロボットに作用する力が所定の許容値を超えないことを保証できる範囲で、最大の動作速度でロボットを減速停止させることができる。この結果、位置ずれ時の適切な停止動作を確保する制約の元で、動作時間を最短にできる。さらに、エラー発生を検知して減速停止させる場合でも作用力が許容値以下となるような最高速度を試行錯誤により求める必要がなくなり、立上げ調整時間を短縮できる。
【００４１】
以上のように、実施の形態１によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる。
【００４２】
さらに、上式（１）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００４３】
実施の形態２．
図５は、本発明の実施の形態２における力制御装置の構成を示すブロック図である。先の実施の形態１における図１の構成と比較すると、本実施の形態２における図５の構成は、対象物特性同定手段５０をさらに備えるとともに、速度最適化手段４０の内部構成が異なっている。そこで、速度最適化手段４０の内部と対象物特性同定手段５０を中心に、以下に説明する。
【００４４】
先の実施の形態１では、対象物特性（対象物剛性）を速度最適化手段４０の内部にあらかじめ記憶しておく必要があった。これに対して、本実施の形態２では、対象物特性同定手段５０を新たに設けることで、立上げ調整時の動作データから対象物剛性を同定しておき、同定した値を記憶しておく。一方、速度最適化手段４０は、対象物特性同定手段５０に記憶された対象物剛性を読み出し、最適速度を算出する際の演算に用いる。
【００４５】
対象物特性同定手段５０による具体的な同定方法としては、次のようなものが考えられる。例えば、対象物の特性を線形ばねと仮定する。この場合、対象物特性同定手段５０は、同定動作として押し込み動作を行ったときの動作データから、接触開始位置からの移動距離と押し込み力の時系列データ、もしくは２箇所以上の移動距離と押し込み力のデータの組合せから、線形同定理論を用いて、ばね定数を同定することができる。
【００４６】
そこで、本実施の形態２における対象物特性同定手段５０は、同定用に実施した同定動作での接触開始位置からの移動距離と押し込み力の時系列データから、対象物の剛性Ｋ１をあらかじめ同定して記憶しておく。そして、速度最適化手段４０は、対象物特性同定手段５０による同定結果に基づいて最適速度を算出することができる。この結果、速度最適化手段４０は、対象物特性をあらかじめ記憶しておく必要がない構成とすることができる。
【００４７】
以上のように、実施の形態２によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる
【００４８】
さらに、上式（１）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。さらに、対象物特性同定手段を備えることで、対象物の特性が未知の場合にも、１回の同定動作を実行することで対象物特性を同定でき、同定時間も含めた立上げ調整時間の短縮が可能となる。
【００４９】
実施の形態３．
図６は、本発明の実施の形態３における力制御装置の構成を示すブロック図である。先の実施の形態１、２における速度最適化手段４０は、コンプライアンス制御パラメータＫ２を用いて、上式（１）により最適速度Ｖｓを算出していた。これに対して、本実施の形態３における速度最適化手段４０は、コンプライアンス制御パラメータＫ２を使用せず、下式（２）により最適速度Ｖｓを算出する。
Ｖｓ＝ｓｑｒｔ（（Ｆｐ−Ｆｓ）×２×ｇｔ／Ｋ１） （２）
【００５０】
すなわち、速度最適化手段４０は、上式（２）を用いて、対象物の特性および所定の力制限値に基づいて、前記減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度Ｖｓを算出する。
【００５１】
さらに、速度最適化手段４０は、算出した最適速度Ｖｓを指令生成手段１０に出力する。その他の処理は、先の実施の形態１と同様である。なお、先の実施の形態２で説明した対象物特性同定手段５０を備えた図５の構成においても、速度最適化手段４０は、上式（２）を用いて最適速度Ｖｓを算出することができる。
【００５２】
以上のように、実施の形態３によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる。
【００５３】
さらに、コンプライアンス制御パラメータＫ２を使用せずに、上式（２）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００５４】
実施の形態４．
図７は、本発明の実施の形態４における力制御装置の構成を示すブロック図である。先の実施の形態１における図１の構成と比較すると、本実施の形態４における図７の構成は、力推定オブザーバ６０をさらに備えており、力覚センサ２を不用としている点が異なっている。
【００５５】
すなわち、本実施の形態４においては、力覚センサ２から力およびモーメントの検出値を得るのではなく、力推定オブザーバ６０を用いて、ロボット先端部に作用する力およびモーメントを推定し、推定した力推定値を力検出値の代わりに使用している。
【００５６】
力推定オブザーバ６０は、各軸のモータ位置と各軸を駆動するモータの電流を入力し、力推定値を推定している。より具体的には、力推定オブザーバ６０は、各軸のモータ位置からモータ速度、モータ加速度を算出し、算出したモータ位置、速度、加速度からその動作で必要となる駆動トルクを算出し、モータ電流から算出される駆動トルクとの差を各軸の外乱トルクとして算出する。
【００５７】
さらに、力推定オブザーバ６０は、各軸のモータ位置に基づいて算出されるヤコビ行列を用いて、外乱トルクを、ロボット先端部に作用する力およびモーメントに変換し、変換した力およびモーメントを力推定値として出力する。
【００５８】
以上のように、実施の形態４によれば、力覚センサを用いることなく、先の実施の形態１、２と同様の効果を得ることができるととともに、力覚センサを不要とすることによるシステムコストの低減を実現できる。
【００５９】
実施の形態５．
図８は、本発明の実施の形態５における力制御装置の構成を示すブロック図である。先の実施の形態４における速度最適化手段４０は、先の実施の形態１、２と同様に、コンプライアンス制御パラメータＫ２を用いて、上式（１）により最適速度Ｖｓを算出していた。これに対して、本実施の形態５における速度最適化手段４０は、先の実施の形態３と同様に、コンプライアンス制御パラメータＫ２を使用せず、上式（２）により最適速度Ｖｓを算出する。その他の処理は、先の実施の形態４と同様である。
【００６０】
以上のように、実施の形態５によれば、力覚センサを用いることなく、先の実施の形態４と同様の効果を得ることができるととともに、力覚センサを不要とすることによるシステムコストの低減を実現できる。さらに、先の実施の形態３と同様に、コンプライアンス制御パラメータＫ２を使用せずに、上式（２）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００６１】
なお、先の実施の形態４、５における力推定オブザーバ６０で推定された力推定値をロボット制御手段内部では使用せずに、位置制御を実施することも可能である。
【符号の説明】
【００６２】
１ ロボット、２ 力覚センサ、３ 位置センサ、１０ 指令生成手段、２０ ロボット制御手段、２１ 力覚考慮補正手段、２２ 逆変換手段、２３ 各軸位置制御手段、２３ａ 比例演算部、２３ｂ 微分演算部、２３ｃ 比例積分演算部、３０ 力制限超過判別手段、４０ 速度最適化手段、５０ 対象物特性同定手段、６０ 力推定オブザーバ、２１１ ツール座標系座標変換部、２１２ スティフネス行列部、２１３ 微分演算部、２１４ ダンピング行列部、２１５ 比例ゲイン演算部、２１６ 直交座標系座標変換部、２１７ 積分演算部、２１８ ツール座標系座標変換部、２１９ 順変換部。
【技術分野】
【０００１】
本発明は、組立、ばりとり、研磨、基板実装、検査などを行うロボット、自動組立装置、実装機、加工機、検査装置などに適用される力制御装置に関する。
【背景技術】
【０００２】
一般的な力制御装置において、部品を収納したマガジンから部品を取り出し、所定位置まで移動して部品組み付け作業を行うロボットの先端部には、力センサが設けられている。ロボットコントローラは、力センサで検出した力のフィードバックによるコンプライアンス制御を行い、検出した力とあらかじめ設定された力の閾値とを比較して、その比較結果により、次のロボット動作の決定、作業成否の判定を行う。
【０００３】
例えば、丸棒状の部品をハンドで把持し、下降し、挿入するといった一連の動作を行う場合、挿入中（下降中）に検出した力が閾値を超えた場合には、ロボットコントローラは、挿入作業が失敗したと判断し、下降動作を停止する。この結果、過大な力の発生を防止することができる。そして、ハンド停止後は、再び作業を行うか、あるいは、異常警報を発生することとなる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
挿入作業の失敗を検知して停止する場合、検知した地点から実際に停止するまでの間に、ロボットは下降する。このため、ロボットおよび対象物には、この下降の間に、検出に用いた閾値以上の力が作用することも考えられる。挿入失敗を検知して停止した際に作用する力（以下の説明においては、このような「作用する力」のことを単に作用力と表現する場合もある）が、ロボットおよび対象物の許容値以下となるようにするには、停止時の作用力が許容値以下となることを保証できる速度以下で下降させる必要がある。
【０００６】
しかしながら、従来の力制御装置は、停止時の作用力が許容値以下となる速度を算出する機能を備えていなかった。このため、必要以上に下降速度を下げる、あるいは、停止時の作用力が許容値以下となる速度を試行錯誤であらかじめ求めておくといった対策が必要であった。しかしながら、前者の場合は、速度が遅いため、動作時間が長くなるといった問題があった。また、後者の場合には、事前調整に時間を要するといった問題があった。
【０００７】
本発明は、前記のような課題を解決するためになされたものであり、挿入失敗等により力制限を超過した際にロボットを動作途中で減速停止する場合でも、作用力が許容値以下となる制約を満たす範囲で動作時間の短縮を図ること、および作用力が許容値以下となる制約を満たす範囲で動作時間の短縮を実現するための調整時間の短縮を図ることのできる力制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る力制御装置は、対象物に対するロボットの目標位置指令を生成する指令生成手段と、目標位置指令に応じてロボットの追従制御を行うロボット制御手段と、追従動作に伴って対象物からロボットへ働く作用力が、対象物もしくはロボットにより決まる作用力の上限値に相当する所定の許容値よりも小さい値として規定される所定の力制限値を超過した場合に、指令生成手段で生成される目標位置指令によりロボットを減速停止させるための停止指令を生成する力制限超過判別手段と、力制限超過判別手段により停止指令が生成されることでロボットを減速停止させる場合を考慮して、対象物の特性および所定の力制限値に基づいて、減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度を算出する速度最適化手段とを備え、指令生成手段は、速度最適化手段で算出された最適速度に応じた目標位置指令を生成するとともに、力制限超過判別手段で生成された停止指令を受信した場合には、ロボットを減速停止させる指令を生成するものである。
【発明の効果】
【０００９】
本発明に係る力制御装置によれば、対象物からロボットへ働く作用力が所定の力制限を超過したことでロボットを減速停止させる必要が生じた際に、減速停止中の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成してロボットを動作させることで、作用力が許容値以下となる制約を満たす範囲でロボット動作時間を短縮でき、ロボット動作時間短縮のための事前の立上げ調整時間の短縮化を図ることのできる力制御装置を得ることができる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施の形態１における力制御装置の構成を示すブロック図である。
【図２】本発明の実施の形態１におけるロボット制御手段の内部構成図である。
【図３】本発明の実施の形態１における各軸位置制御手段の構成図の一例である。
【図４】本発明の実施の形態１における力覚考慮補正手段の内部構成図である。
【図５】本発明の実施の形態２における力制御装置の構成を示すブロック図である。
【図６】本発明の実施の形態３における力制御装置の構成を示すブロック図である。
【図７】本発明の実施の形態４における力制御装置の構成を示すブロック図である。
【図８】本発明の実施の形態５における力制御装置の構成を示すブロック図である。
【発明を実施するための形態】
【００１１】
以下、本発明の力制御装置の好適な実施の形態につき図面を用いて説明する。
【００１２】
実施の形態１．
図１は、本発明の実施の形態１における力制御装置の構成を示すブロック図である。本実施の形態１における力制御装置は、指令生成手段１０、ロボット制御手段２０、力制限超過判別手段３０、および速度最適化手段４０を備えており、制御対象であるロボット１の追従制御を行っている。このロボット１は、１軸もしくは複数軸のモータを備えるものであり、その一例として、産業用ロボットと呼ばれる６軸垂直多関節ロボット、あるいは４軸水平多関節ロボットが挙げられる。
【００１３】
指令生成手段１０は、ロボット１に時々刻々の目標位置を与える。ロボット制御手段２０は、指令生成手段１０から与えられた位置指令に応じて、ロボット１を追従制御する。
【００１４】
ロボット１の先端部近辺もしくはロボット１が作業する対象には、作用する力とモーメントを計測するための力覚センサ２が取り付けられている。そして、力覚センサ２で測定された力およびモーメントは、力検出値としてロボット制御手段２０に入力されるとともに、力制限超過判別手段３０にも入力される。
【００１５】
力検出値は、６軸の力覚センサ２を用いている場合には、３軸方向の力と各軸周りのモーメントで構成され、５軸以下の力覚センサ２を用いている場合には、３軸方向の力と各軸周りのモーメントのうち、計測できるデータのみで構成される。
【００１６】
また、ロボット１を駆動する各軸のモータには、エンコーダやレゾルバなどの各軸のモータ位置を測定する位置センサ３がそれぞれ取り付けられており、こうした位置センサ３で測定した各軸のモータ位置は、ロボット制御手段２０に入力される。
【００１７】
力制限超過判別手段３０は、軸ごとにあらかじめ記憶された所定の力閾値と力覚センサ２から送信される力検出値とを比較し、少なくとも１つの軸方向の力もしくはモーメントが力閾値を超えた場合には、指令生成手段１０に停止指令を送信する。なお、本実施の形態１では、力制限超過の判別を、検出した力とモーメントの各成分に対してそれぞれ所定の力閾値と比較することで実施しているが、合成した力やモーメントに対して実施してもよい。
【００１８】
速度最適化手段４０は、力制限超過判別手段３０から入力される力制限値（力閾値）、ロボット制御手段２０から入力されるコンプライアンス制御パラメータ、および速度最適化手段４０の内部にあらかじめ設定されている力許容値、対象物の剛性に関するパラメータ（対象物のばね定数）に基づいて、最適速度を求め、指令生成手段１０に出力する。
【００１９】
ここで、力許容値は、力制限値よりも大きい値であり、対象物の制約もしくはロボットの制約の低い方により決まる絶対超えてはいけない作用力の上限値であり、この値を超過することがないようにロボットを制御する値に相当する。一方、力制限値は、力許容値以下となる制約を満たすために設ける制限値で、力制限値を超えたらロボットを減速停止させることで、作用力が力許容値を超えることを防止しようとするものである。
【００２０】
また、速度最適化手段４０が出力する「最適速度」とは、力制限超過判別手段３０で力制限超過を判別し停止する場合に、対象物およびロボット１に作用する力およびモーメントが、速度最適化手段４０の内部に設定されている力許容値以下となる上限速度を意味している。
【００２１】
指令生成手段１０は、速度最適化手段４０から入力された最適速度を最高速度とし、指令生成手段１０の内部に記憶されている加減速パラメータに基づいて、指令生成周期ごとに位置指令を生成し、ロボット制御手段２０に出力する。また、指令生成手段１０は、力制限超過判別手段３０から停止指令を受け取ると、現在の位置・速度から減速停止する指令を生成して、ロボット制御手段２０に送信する。
【００２２】
次に、ロボット制御手段２０の機能について、詳細に説明する。図２は、本発明の実施の形態１におけるロボット制御手段２０の内部構成図である。図２におけるロボット制御手段２０は、力覚考慮補正手段２１、逆変換手段２２、および各軸位置制御手段２３で構成されている。
【００２３】
力覚考慮補正手段２１は、指令生成手段１０で生成された位置指令に基づいて、位置指令修正量を生成する。この位置指令修正量の生成の詳細に関しては、後述する。そして、位置指令は、力覚考慮補正手段２１の出力である位置指令修正量を加算することで補正される。
【００２４】
次に、逆変換手段２２は、位置指令と位置指令修正量との加算値に対して、手先位置姿勢を実現する各軸のモータ位置を算出する逆変換計算を行い、ロボットの各軸の位置指令である関節位置指令に変換する。
【００２５】
変換した関節位置指令は、各軸位置制御手段２３に入力される。各軸位置制御手段２３は、入力された関節位置指令に各軸のモータ位置が追従するように制御を行う。図３は、本発明の実施の形態１における各軸位置制御手段２３の構成図の一例である。
【００２６】
図３に示す各軸位置制御手段２３の内部は、それぞれの軸（１軸〜ｎ軸）が、比例演算部２３ａ、微分演算部２３ｂ、および比例積分演算部２３ｃで構成されている。符号の後ろの（）内の数字は、軸の番号に対応している。
【００２７】
比例演算部２３ａは、各軸の位置指令と当該軸のモータ位置との差に対して比例演算を行う。一方、微分演算部２３ｂは、モータ位置を微分することでモータ速度を算出する。そして、比例積分演算部２３ｃは、比例演算部２３ａの出力から微分演算部２３ｂの出力を減算した値に対して比例積分演算を行う。
【００２８】
上記制御系は、位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。なお、図３に示した一例では、各軸位置制御手段２３の構成として、各軸ごとにモータ位置制御と速度制御を行うフィードバック制御系の構成とした。しかしながら、本発明の制御系は、このような構成に限定されるものではない。各軸ごとにオブザーバを含む制御系であってもよいし、フィードフォワードを含む制御系であっても構わない。また、全て各軸独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であっても構わない。
【００２９】
次に、力覚考慮補正手段２１の内部構成について、詳細に説明する。図４は、本発明の実施の形態１における力覚考慮補正手段２１の内部構成図である。図４に示すように、本実施の形態１における力覚考慮補正手段２１は、ツール座標系座標変換部２１１、スティフネス行列部２１２、微分演算部２１３、ダンピング行列部２１４、比例ゲイン演算部２１５、直交座標系座標変換部２１６、積分演算部２１７、ツール座標系座標変換部２１８、および順変換部２１９を備えている。
【００３０】
まず、順変換部２１９は、各軸モータ位置からそのときの手先位置姿勢を算出する順変換を行い、手先位置姿勢を算出する。ツール座標系座標変換部２１１は、指令生成手段１０で生成された位置指令（手先位置姿勢の指令）と、順変換部２１９で算出された手先位置姿勢との差を入力として、ツール座標系への座標変換を行う。ここで、「ツール座標系」とは、ロボット先端に取り付けたツールやハンドに固定された座標系を意味する。
【００３１】
次に、スティフネス行列部２１２は、ツール座標系座標変換部２１１の出力から構成されるベクトルを入力し、スティフネス行列とのベクトル積を求め、その積を出力する。
【００３２】
一方、微分演算部２１３は、ツール座標系座標変換部２１１の出力の各要素の微分を算出する。そして、ダンピング行列部２１４は、微分演算部２１３で算出された微分結果から構成されるベクトルと、ダンピング行列とのベクトル積を求め、その積を出力する。さらに、スティフネス行列部２１２の出力と、ダンピング行列部２１４の出力とが加算される。
【００３３】
次に、ツール座標系座標変換部２１８は、力覚センサ２で検出された力検出値をツール座標系に変換する。そして、ツール座標系座標変換部２１８により変換された力検出値は、先ほどのスティフネス行列部２１２の出力とダンピング行列部２１４の出力との加算結果から減算され、比例ゲイン演算部２１５に入力される。
【００３４】
比例ゲイン演算部２１５は、入力した減算結果の各要素に対して比例ゲインを乗じる。さらに、直交座標系座標変換部２１６は、比例ゲインの乗算結果を直交座標系に座標変換する。さらに、積分演算部２１７は、直交座標系座標変換部２１６による変換結果に対して積分処理を行い、位置指令修正量（位置指令補正量）を算出し、出力する。このような一連処理により、力覚考慮補正手段２１は、指令生成手段１０で生成された位置指令に基づいて、位置指令修正量を生成することができる。
【００３５】
次に、速度最適化手段４０の内部構成について、詳細に説明する。なお、本実施の形態１では、力制限値（力閾値）と力許容値とを各軸毎に設定する場合を例に挙げて説明するが、各軸方向の力およびモーメントの合成で考えても構わない。また、以下では、代表としてＺ軸方向について説明するが、Ｘ軸方向、Ｙ軸方向の力を考える場合も、同様である。
【００３６】
Ｚ軸方向の力閾値（力制限値）をＦｓ、Ｚ軸方向の力許容値をＦｐ、Ｚ方向の対象物の剛性をＫ１、Ｚ方向のコンプライアンス制御パラメータをＫ２とする。ここで、Ｆｓは、力制限超過判別手段３０から入力される力閾値であり、作用する力がＦｓを超えると、力制限超過判別手段３０から指令生成手段１０に対して、停止指令が入力され、ロボット１は、減速停止する。
【００３７】
また、対象物の剛性Ｋ１および力許容値Ｆｐは、あらかじめ速度最適化手段４０の内部に記憶されている。速度最適化手段４０は、それぞれの軸の動作の直前に、力制限超過判別手段３０から入力される力閾値Ｆｓ、ロボット制御手段２０から入力されるコンプライアンス制御パラメータＫ２（スティフネス行列のＺ軸に対応する対角要素）、あらかじめ記憶されている対象物の許容力Ｆｐ、対象物の剛性Ｋ１、および減速時間ｇｔから、下式（１）により、最適速度Ｖｓを算出する。
Ｖｓ＝ｓｑｒｔ（（Ｆｐ−Ｆｓ）×（Ｋ１＋Ｋ２）×２×ｇｔ／Ｋ１／Ｋ２） （１）
【００３８】
すなわち、速度最適化手段４０は、上式（１）を用いて、対象物の特性および所定の力制限値に基づいて、前記減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度Ｖｓを算出する。
【００３９】
そして、速度最適化手段４０は、算出した最適速度Ｖｓを指令生成手段１０に出力する。なお、上式（１）において、ｓｑｒｔは、平方根を意味している。指令生成手段１０は、最高速度がＶｓとなるように、指令生成周期ごとに位置指令を生成し、ロボット制御手段２０に出力する。
【００４０】
このような制御を行うことで、位置ずれなどにより正常な場合と比べて過大な力がロボットに作用した際に（エラー発生時に相当）、ロボットを減速停止させた場合においても、減速停止中にロボットに作用する力が所定の許容値を超えないことを保証できる範囲で、最大の動作速度でロボットを減速停止させることができる。この結果、位置ずれ時の適切な停止動作を確保する制約の元で、動作時間を最短にできる。さらに、エラー発生を検知して減速停止させる場合でも作用力が許容値以下となるような最高速度を試行錯誤により求める必要がなくなり、立上げ調整時間を短縮できる。
【００４１】
以上のように、実施の形態１によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる。
【００４２】
さらに、上式（１）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００４３】
実施の形態２．
図５は、本発明の実施の形態２における力制御装置の構成を示すブロック図である。先の実施の形態１における図１の構成と比較すると、本実施の形態２における図５の構成は、対象物特性同定手段５０をさらに備えるとともに、速度最適化手段４０の内部構成が異なっている。そこで、速度最適化手段４０の内部と対象物特性同定手段５０を中心に、以下に説明する。
【００４４】
先の実施の形態１では、対象物特性（対象物剛性）を速度最適化手段４０の内部にあらかじめ記憶しておく必要があった。これに対して、本実施の形態２では、対象物特性同定手段５０を新たに設けることで、立上げ調整時の動作データから対象物剛性を同定しておき、同定した値を記憶しておく。一方、速度最適化手段４０は、対象物特性同定手段５０に記憶された対象物剛性を読み出し、最適速度を算出する際の演算に用いる。
【００４５】
対象物特性同定手段５０による具体的な同定方法としては、次のようなものが考えられる。例えば、対象物の特性を線形ばねと仮定する。この場合、対象物特性同定手段５０は、同定動作として押し込み動作を行ったときの動作データから、接触開始位置からの移動距離と押し込み力の時系列データ、もしくは２箇所以上の移動距離と押し込み力のデータの組合せから、線形同定理論を用いて、ばね定数を同定することができる。
【００４６】
そこで、本実施の形態２における対象物特性同定手段５０は、同定用に実施した同定動作での接触開始位置からの移動距離と押し込み力の時系列データから、対象物の剛性Ｋ１をあらかじめ同定して記憶しておく。そして、速度最適化手段４０は、対象物特性同定手段５０による同定結果に基づいて最適速度を算出することができる。この結果、速度最適化手段４０は、対象物特性をあらかじめ記憶しておく必要がない構成とすることができる。
【００４７】
以上のように、実施の形態２によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる
【００４８】
さらに、上式（１）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。さらに、対象物特性同定手段を備えることで、対象物の特性が未知の場合にも、１回の同定動作を実行することで対象物特性を同定でき、同定時間も含めた立上げ調整時間の短縮が可能となる。
【００４９】
実施の形態３．
図６は、本発明の実施の形態３における力制御装置の構成を示すブロック図である。先の実施の形態１、２における速度最適化手段４０は、コンプライアンス制御パラメータＫ２を用いて、上式（１）により最適速度Ｖｓを算出していた。これに対して、本実施の形態３における速度最適化手段４０は、コンプライアンス制御パラメータＫ２を使用せず、下式（２）により最適速度Ｖｓを算出する。
Ｖｓ＝ｓｑｒｔ（（Ｆｐ−Ｆｓ）×２×ｇｔ／Ｋ１） （２）
【００５０】
すなわち、速度最適化手段４０は、上式（２）を用いて、対象物の特性および所定の力制限値に基づいて、前記減速停止中において作用力が所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度Ｖｓを算出する。
【００５１】
さらに、速度最適化手段４０は、算出した最適速度Ｖｓを指令生成手段１０に出力する。その他の処理は、先の実施の形態１と同様である。なお、先の実施の形態２で説明した対象物特性同定手段５０を備えた図５の構成においても、速度最適化手段４０は、上式（２）を用いて最適速度Ｖｓを算出することができる。
【００５２】
以上のように、実施の形態３によれば、動作直前に力制限を超過したことを判別して、停止した際の作用力が許容値以下となる制約を満たす範囲での最高速度を算出し、算出した最高速度に基づいて位置指令を生成し、ロボットを動作させる。この結果、力制限を超過したエラー発生時にロボットを減速停止させる必要が生じた際に、作用力を許容値以下に抑えながら動作時間の短縮化を図ることができる。
【００５３】
さらに、コンプライアンス制御パラメータＫ２を使用せずに、上式（２）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００５４】
実施の形態４．
図７は、本発明の実施の形態４における力制御装置の構成を示すブロック図である。先の実施の形態１における図１の構成と比較すると、本実施の形態４における図７の構成は、力推定オブザーバ６０をさらに備えており、力覚センサ２を不用としている点が異なっている。
【００５５】
すなわち、本実施の形態４においては、力覚センサ２から力およびモーメントの検出値を得るのではなく、力推定オブザーバ６０を用いて、ロボット先端部に作用する力およびモーメントを推定し、推定した力推定値を力検出値の代わりに使用している。
【００５６】
力推定オブザーバ６０は、各軸のモータ位置と各軸を駆動するモータの電流を入力し、力推定値を推定している。より具体的には、力推定オブザーバ６０は、各軸のモータ位置からモータ速度、モータ加速度を算出し、算出したモータ位置、速度、加速度からその動作で必要となる駆動トルクを算出し、モータ電流から算出される駆動トルクとの差を各軸の外乱トルクとして算出する。
【００５７】
さらに、力推定オブザーバ６０は、各軸のモータ位置に基づいて算出されるヤコビ行列を用いて、外乱トルクを、ロボット先端部に作用する力およびモーメントに変換し、変換した力およびモーメントを力推定値として出力する。
【００５８】
以上のように、実施の形態４によれば、力覚センサを用いることなく、先の実施の形態１、２と同様の効果を得ることができるととともに、力覚センサを不要とすることによるシステムコストの低減を実現できる。
【００５９】
実施の形態５．
図８は、本発明の実施の形態５における力制御装置の構成を示すブロック図である。先の実施の形態４における速度最適化手段４０は、先の実施の形態１、２と同様に、コンプライアンス制御パラメータＫ２を用いて、上式（１）により最適速度Ｖｓを算出していた。これに対して、本実施の形態５における速度最適化手段４０は、先の実施の形態３と同様に、コンプライアンス制御パラメータＫ２を使用せず、上式（２）により最適速度Ｖｓを算出する。その他の処理は、先の実施の形態４と同様である。
【００６０】
以上のように、実施の形態５によれば、力覚センサを用いることなく、先の実施の形態４と同様の効果を得ることができるととともに、力覚センサを不要とすることによるシステムコストの低減を実現できる。さらに、先の実施の形態３と同様に、コンプライアンス制御パラメータＫ２を使用せずに、上式（２）に基づいて動作速度を自動設定できるため、試行錯誤により動作速度を最適化する必要がなく、立上げ時間の短縮化を実現できる。
【００６１】
なお、先の実施の形態４、５における力推定オブザーバ６０で推定された力推定値をロボット制御手段内部では使用せずに、位置制御を実施することも可能である。
【符号の説明】
【００６２】
１ ロボット、２ 力覚センサ、３ 位置センサ、１０ 指令生成手段、２０ ロボット制御手段、２１ 力覚考慮補正手段、２２ 逆変換手段、２３ 各軸位置制御手段、２３ａ 比例演算部、２３ｂ 微分演算部、２３ｃ 比例積分演算部、３０ 力制限超過判別手段、４０ 速度最適化手段、５０ 対象物特性同定手段、６０ 力推定オブザーバ、２１１ ツール座標系座標変換部、２１２ スティフネス行列部、２１３ 微分演算部、２１４ ダンピング行列部、２１５ 比例ゲイン演算部、２１６ 直交座標系座標変換部、２１７ 積分演算部、２１８ ツール座標系座標変換部、２１９ 順変換部。
【特許請求の範囲】
【請求項１】
対象物に対するロボットの目標位置指令を生成する指令生成手段と、
前記目標位置指令に応じて前記ロボットの追従制御を行うロボット制御手段と、
追従動作に伴って前記対象物から前記ロボットへ働く作用力が、前記対象物もしくは前記ロボットにより決まる作用力の上限値に相当する所定の許容値よりも小さい値として規定される所定の力制限値を超過した場合に、前記指令生成手段で生成される前記目標位置指令により前記ロボットを減速停止させるための停止指令を生成する力制限超過判別手段と、
前記力制限超過判別手段により前記停止指令が生成されることで前記ロボットを減速停止させる場合を考慮して、前記対象物の特性および前記所定の力制限値に基づいて、前記減速停止中において前記作用力が前記所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度を算出する速度最適化手段と
を備え、
前記指令生成手段は、前記速度最適化手段で算出された前記最適速度に応じた目標位置指令を生成するとともに、前記力制限超過判別手段で生成された前記停止指令を受信した場合には、前記ロボットを減速停止させる指令を生成する
ことを特徴とする力制御装置。
【請求項２】
請求項１に記載の力制御装置において、
前記ロボットが同定動作を行うことにより得られる時系列データに基づいて、前記対象物の特性を同定する対象物特性同定手段をさらに備え、
前記ロボット制御手段は、前記対象物の特性を同定するためにあらかじめ実施した同定動作に伴って、前記作用力、および前記ロボットの各軸モータの位置情報を取得し、前記ロボットが前記対象物に接触を開始してからの移動距離と、前記移動距離に対応する前記作用力の時系列データに基づいて、前記対象物の特性を同定し、
前記速度最適化手段は、前記対象物特性同定手段により同定された前記対象物の特性を用いて、前記最適速度を算出する
ことを特徴とする力制御装置。
【請求項３】
請求項１または２に記載の力制御装置において、
前記ロボットを駆動するための各軸モータの電流値および位置情報に基づいて前記作用力を推定する力推定オブザーバをさらに備え、
前記力制限超過判別手段は、前記力推定オブザーバで推定された前記作用力が前記所定の力制限値を超過している場合に、前記停止指令を生成する
ことを特徴とする力制御装置。
【請求項１】
対象物に対するロボットの目標位置指令を生成する指令生成手段と、
前記目標位置指令に応じて前記ロボットの追従制御を行うロボット制御手段と、
追従動作に伴って前記対象物から前記ロボットへ働く作用力が、前記対象物もしくは前記ロボットにより決まる作用力の上限値に相当する所定の許容値よりも小さい値として規定される所定の力制限値を超過した場合に、前記指令生成手段で生成される前記目標位置指令により前記ロボットを減速停止させるための停止指令を生成する力制限超過判別手段と、
前記力制限超過判別手段により前記停止指令が生成されることで前記ロボットを減速停止させる場合を考慮して、前記対象物の特性および前記所定の力制限値に基づいて、前記減速停止中において前記作用力が前記所定の許容値以下になる制約を満たす範囲で、動作速度を最大にする最適速度を算出する速度最適化手段と
を備え、
前記指令生成手段は、前記速度最適化手段で算出された前記最適速度に応じた目標位置指令を生成するとともに、前記力制限超過判別手段で生成された前記停止指令を受信した場合には、前記ロボットを減速停止させる指令を生成する
ことを特徴とする力制御装置。
【請求項２】
請求項１に記載の力制御装置において、
前記ロボットが同定動作を行うことにより得られる時系列データに基づいて、前記対象物の特性を同定する対象物特性同定手段をさらに備え、
前記ロボット制御手段は、前記対象物の特性を同定するためにあらかじめ実施した同定動作に伴って、前記作用力、および前記ロボットの各軸モータの位置情報を取得し、前記ロボットが前記対象物に接触を開始してからの移動距離と、前記移動距離に対応する前記作用力の時系列データに基づいて、前記対象物の特性を同定し、
前記速度最適化手段は、前記対象物特性同定手段により同定された前記対象物の特性を用いて、前記最適速度を算出する
ことを特徴とする力制御装置。
【請求項３】
請求項１または２に記載の力制御装置において、
前記ロボットを駆動するための各軸モータの電流値および位置情報に基づいて前記作用力を推定する力推定オブザーバをさらに備え、
前記力制限超過判別手段は、前記力推定オブザーバで推定された前記作用力が前記所定の力制限値を超過している場合に、前記停止指令を生成する
ことを特徴とする力制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−２３５４２３（Ｐ２０１１−２３５４２３Ａ）
【公開日】平成２３年１１月２４日（２０１１．１１．２４）
【国際特許分類】
【出願番号】特願２０１０−１１０９１７（Ｐ２０１０−１１０９１７）
【出願日】平成２２年５月１３日（２０１０．５．１３）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２１年度、独立行政法人新エネルギー・産業技術総合開発機構、「戦略的先端ロボット要素技術開発プロジェクト、柔軟物も取扱える生産用ロボットシステム（次世代産業用ロボット分野）、ＦＡ機器組立ロボットシステムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年１１月２４日（２０１１．１１．２４）
【国際特許分類】
【出願日】平成２２年５月１３日（２０１０．５．１３）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２１年度、独立行政法人新エネルギー・産業技術総合開発機構、「戦略的先端ロボット要素技術開発プロジェクト、柔軟物も取扱える生産用ロボットシステム（次世代産業用ロボット分野）、ＦＡ機器組立ロボットシステムの研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]