(57)【要約】【課題】ロボットアーム先端部分の移動方向を比較的大きく変化させる場合であっても、ワークをより正確に加工する。【解決手段】ロボットアーム（５０ａ）の先端に取付けられた作業ツール（５２）とワーク（６０）との間に作用する力を制御するロボット制御装置（１０）が、制御をおこなう際に必要な力データを取得する力測定手段（５１）と、作業ツールから前記力測定手段に及ぼされる力のうち、重力が作用する力と前記ロボットアームの運動により発生する動力学項とを算出する算出手段と、算出手段により算出された、前記重力が作用する力と前記動力学項とを用いて、前記力測定手段により測定された力を補正する補正手段（１４）と、動力学項が所定の閾値より大きい場合には、前記動力学項および前記重力により前記力測定手段に及ぼされる力に応じて、前記ロボットアームの動作指令を調整する指令調整手段（１２）と、を具備する。【選択図】図２
2008142810.tif 000002
【特許請求の範囲】
【請求項１】
ロボットアームの先端に取付けられた作業ツールとワークとの間に作用する力を制御するロボット制御装置において、
前記制御をおこなう際に必要な力データを取得する力測定手段と、
前記作業ツールから前記力測定手段に及ぼされる力のうち、重力が前記力測定手段に作用する力と前記ロボットアームの運動により発生する動力学項とを算出する算出手段と、
前記算出手段により算出された、前記重力が前記力測定手段に作用する力と前記動力学項とを用いて、前記力測定手段により測定された力を補正する補正手段と、
前記動力学項が所定の閾値より大きい場合には、前記動力学項および前記重力により前記力測定手段に及ぼされる力に応じて、前記ロボットアームの動作指令を調整する指令調整手段と、を具備するロボット制御装置。
【請求項２】
ロボットアームの先端に取付けられたハンドにより把持されたワークと固定された作業ツールとの間に作用する力を制御するロボット制御装置において、
前記制御をおこなう際に必要な力データを取得する力測定手段と、
前記ワークを把持した前記ハンドから前記力測定手段に及ぼされる力のうち、重力が前記力測定手段に作用する力と前記ロボットアームの運動により発生する動力学項とを算出する算出手段と、
前記算出手段により算出された、前記重力が前記力測定手段に作用する力と前記動力学項とを用いて、前記力測定手段により測定された力を補正する補正手段と、
前記動力学項が所定の閾値より大きい場合には、前記動力学項および前記重力により前記力測定手段に及ぼされる力に応じて、前記ロボットアームの動作指令を調整する指令調整手段と、を具備するロボット制御装置。
【請求項３】
前記指令調整手段により調整される前記ロボットアームの前記動作指令は、前記作業ツールと前記ワークとの間に作用すべき力の指令を含む請求項１または２に記載のロボット制御装置。
【請求項４】
前記指令調整手段により調整される前記ロボットアームの前記動作指令は、前記ロボットアームの進行方向速度を含む請求項１から３のいずれか一項に記載のロボット制御装置。
【請求項５】
前記指令調整手段により調整される前記ロボットアームの前記動作指令は、ロボットアームの目標軌道を含む請求項１から４のいずれか一項に記載のロボット制御装置。
【請求項６】
前記指令調整手段により調整される前記ロボットアームの前記動作指令は、力制御のゲインを含む請求項１から５のいずれか一項に記載のロボット制御装置。
【請求項７】
前記算出手段は、前記ロボットアームの姿勢、動作速度や動作加速度と、前記ロボットアーム先端部に取付けられたエンドエフェクタ部の質量と重心から、動力学項と重力とを算出する請求項１から６のいずれか一項に記載のロボット制御装置。
【請求項８】
前記力測定手段は力覚センサである、請求項１から７のいずれか一項に記載のロボット制御装置。
【請求項９】
前記力測定手段は前記ロボットアームを駆動するアクチュエータの電流値に基づいて力を推定する請求項１から７のいずれか一項に記載のロボット制御装置。
【請求項１０】
前記ロボット制御装置により行われる力制御はインピーダンス制御である請求項１から９のいずれか一項に記載のロボット制御装置。
【請求項１１】
前記ロボット制御装置により行われる力制御はダンピング制御である請求項１から９のいずれか一項に記載のロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置に関する。特に、本発明は、ロボットによってバリ取り、研削、研磨等の加工作業を制御するロボット制御装置に関する。
【背景技術】
【０００２】
ロボットによりバリ取り、研磨、研削等の加工作業を行うために、ロボットアーム先端部分に取付けた作業ツールを適切な力でワークに押付けながら作業ツールを移動させることが行われている。同様な目的のために、ロボットアーム先端部分に取付けたハンドにより把持されたワークを適切な力で固定された加工機械に押付けながらハンドを移動させる場合もある。
【０００３】
このとき、ワークと作業ツールとの間に働く力が適切な値になるように、コンプライアンス機構を有する作業ツールまたはそのような加工機械を使用することがある。また、力覚センサまたは機械駆動部のアクチュエータの電流値を用いることによりワークと作業ツールとの間に働く力を推定し、この力をフィードバックして作業ツールの位置または力を制御する場合もある。
【０００４】
力測定手段、例えば力覚センサにより検出される力には、作業ツールとワークとの間に作用する正味の力以外の力、例えばロボットのエンドエフェクタの重力およびジャイロモーメントによる外力も含まれている。特許文献１においては、力測定手段により検出された力から重力およびジャイロモーメントによる外力を減算することにより、力を補正することが開示されている。
【０００５】
さらに、倣い加工作業においてロボットアーム先端部分の移動方向を比較的大きく変更するときには、ロボットによる押付力の制御が不完全になり、ワークの加工が不十分または過剰になる場合がある。この問題を解決するために、特許文献２においては、予め定義したプログラムに従ってコーナ部における予め指定した所定範囲内においてエンドエフェクタの進行方向速度を減少させると共に目標押付力を減少させるようにしている。また、特許文献３および特許文献４には、加工経路上の曲率を算出して、算出された曲率に応じてロボットとワークとの間の相対速度や目標押付力を決定することが開示されている。
【特許文献１】特許第３４１２２３６号
【特許文献２】特許第３４０２８２９号
【特許文献３】特許第２７３９７６４号
【特許文献４】特開平５−１４３１４６号
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、実際には、力測定手段により検出された力には、ロボットアーム先端のエンドエフェクタがロボットアームと共に運動することにより発生する遠心力、コリオリ力といった慣性力などの動力学項も含まれている。
【０００７】
さらに、倣い加工作業において、ロボットアーム先端部分の移動方向を比較的大きく変化させる場合には、動力学項の値も比較的大きくなる。そのため、作業ツールとワークとの間に作用する正味の力をより正確に推定するためには、この動力学項を考慮する必要がある。
【０００８】
重力および動力学項の値はロボットの姿勢または作業ツールの質量などから推定するが、これらの値には誤差が必ず含まれるので完璧な推定をすることはできない。ロボットアーム先端部分の移動方向を比較的大きく変化させる場合には、動力学項が大きくなるので推定誤差も大きくなる傾向にある。また、制御系の遅れや追従性能の限界によって、力の補正だけではワークに対して所望の加工状態を実現できない事態も生じうる。
【０００９】
このような問題に対処するためには、前述した特許文献２等における、進行方向速度および目標押付力を減少させるという方法が有効になり得る。しかしながら、特許文献２においては、ワークの表面形状が既知であると共に進行方向速度および押付力を変化させる領域を予め指定する必要がある。
【００１０】
また、特許文献３および特許文献４においては、進行方向速度または目標押付力を変化させる判断基準として、曲率などの指標を別に算出する必要がある。さらに、特許文献３および特許文献４においては、作業ツールまたはハンド、もしくは把持するワークが異なる場合にはエンドエフェクタの質量、重心位置が変わるので、曲率が同じ値になる場合、例えば同一の軌道で動作させる場合であっても、ワークまたはエンドエフェクタの種類ごとに進行方向速度および押付力の調整値を変更する必要がある。
【００１１】
本発明はこのような事情に鑑みてなされたものであり、ロボットアーム先端部分の移動方向を比較的大きく変化させる場合であっても、ワークをより正確に加工することのできるロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１２】
前述した目的を達成するために１番目の発明によれば、ロボットアームの先端に取付けられた作業ツールとワークとの間に作用する力を制御するロボット制御装置において、前記制御をおこなう際に必要な力データを取得する力測定手段と、前記作業ツールから前記力測定手段に及ぼされる力のうち、重力が前記力測定手段に作用する力と前記ロボットアームの運動により発生する動力学項とを算出する算出手段と、前記算出手段により算出された、前記重力が前記力測定手段に作用する力と前記動力学項とを用いて、前記力測定手段により測定された力を補正する補正手段と、前記動力学項が所定の閾値より大きい場合には、前記動力学項および前記重力により前記力測定手段に及ぼされる力に応じて、前記ロボットアームの動作指令を調整する指令調整手段と、を具備するロボット制御装置が提供される。
【００１３】
さらに、２番目の発明によれば、ロボットアームの先端に取付けられたハンドにより把持されたワークと固定された作業ツールとの間に作用する力を制御するロボット制御装置において、前記制御をおこなう際に必要な力データを取得する力測定手段と、前記ワークを把持した前記ハンドから前記力測定手段に及ぼされる力のうち、重力が前記力測定手段に作用する力と前記ロボットアームの運動により発生する動力学項とを算出する算出手段と、前記算出手段により算出された、前記重力が前記力測定手段に作用する力と前記動力学項とを用いて、前記力測定手段により測定された力を補正する補正手段と、前記動力学項が所定の閾値より大きい場合には、前記動力学項および前記重力により前記力測定手段に及ぼされる力に応じて、前記ロボットアームの動作指令を調整する指令調整手段と、を具備するロボット制御装置が提供される。
【００１４】
すなわち１番目および２番目の発明においては、先端部分の移動方向が比較的大きく変化するような倣い加工動作を行う場合であっても、ロボットアーム先端部分のエンドエフェクタ、例えば作業ツールまたはハンドの重量と、ロボットアームの運動による動力学項、例えば遠心力、コリオリ力などの慣性力等の補正を行うことができる。従って、作業ツールとワークとの間に作用する正味の力の推定誤差を可能な限り小さくできる。このため、本発明においては、作業ツールとワークとの間に作用する押付力をより正確に制御することができる。
【００１５】
さらに、１番目および２番目の発明においては、ロボットアームの動作に関連する指令を調整する際に、加工経路上の調整箇所を予め教示する必要はなく、また加工経路上の曲率を別途算出する必要もない。さらに、１番目および２番目の発明においては、重力および動力学項から、加工が不十分または過剰になりそうであるかを判断し、それに応じて動作指令を調節することができる。また、閾値は、設定した目標押付力に基づいて定めるのが好ましく、それにより、作業ツール、ハンドまたはワークが取替えられた場合であっても同一の調整値を使用することができる。
【００１６】
３番目の発明によれば、１番目または２番目の発明において、前記指令調整手段により調整される前記ロボットアームの前記動作指令は、前記作業ツールと前記ワークとの間に作用すべき力の指令を含む。
すなわち３番目の発明においては、動力学項から得られた慣性力と作用方向に応じて力の目標値を変更することによって、ワークを所望の状態に加工することができる。
【００１７】
４番目の発明によれば、１番目から３番目のいずれかの発明において、前記指令調整手段により調整される前記ロボットアームの前記動作指令は、前記ロボットアームの進行方向速度を含む。
すなわち４番目の発明においては、動力学項から得られた慣性力と作用方向に応じて進行方向速度の目標値を変更することによって、ワークを所望の状態に加工することができる。
【００１８】
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記指令調整手段により調整される前記ロボットアームの前記動作指令は、ロボットアームの目標軌道を含む。
すなわち５番目の発明においては、動力学項から得られた慣性力と作用方向に応じて目標軌道を変更することによって、ワークを所望の状態に加工することができる。
【００１９】
６番目の発明によれば、１番目から５番目のいずれかの発明において、前記指令調整手段により調整される前記ロボットアームの前記動作指令は、力制御のゲインを含む。
すなわち６番目の発明においては、動力学項から得られた慣性力と作用方向に応じて力制御のゲインを変更することによって、ワークを所望の状態に加工することができる。
【００２０】
７番目の発明によれば、１番目から６番目のいずれかの発明において、前記算出手段は、前記ロボットアームの姿勢、動作速度や動作加速度と、前記ロボットアーム先端部に取付けられたエンドエフェクタ部の質量と重心から、動力学項と重力とを算出する。
【００２１】
８番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定手段は力覚センサである。
９番目の発明によれば、１番目から７番目のいずれかの発明において、前記力測定手段は前記ロボットアームを駆動するアクチュエータの電流値に基づいて力を推定する。
【００２２】
すなわち８番目および９番目の発明においては、比較的簡易な手法により、力を測定することができる。また、９番目の発明においては、力覚センサを別途準備する必要がない。
【００２３】
１０番目の発明によれば、１番目から９番目のいずれかの発明において、前記ロボット制御装置により行われる力制御はインピーダンス制御である。
１１番目の発明によれば、１番目から９番目のいずれかの発明において、前記ロボット制御装置により行われる力制御はダンピング制御である。
【発明を実施するための最良の形態】
【００２４】
以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボット制御装置により制御されるロボットの斜視図である。図１に示されるロボット５０は、６自由度の多関節型ロボットである。このロボット５０はロボット制御装置１０により制御される。ロボット５０のロボットアーム５０ａの先端には作業ツール５２が取付けられている。図１に示される実施形態においては、作業ツール５２は例えばグラインダである。図１に示される作業ツール５２、例えばグラインダは作業台６１上に予め固定されたワーク６０を研磨するのに使用される。
【００２５】
さらに、力覚センサ５１がロボットアーム５０ａの先端と作業ツール５２との間に配置されている。力覚センサ５１は、作業ツール５２がワーク６０に押付けられるときの力を測定する。ここで、本願明細書において使用される用語「力」は、力とモーメントとの両方を含むものとする。図示されるように力覚センサ５１はロボット制御装置１０に接続されており、測定された力データはロボット制御装置１０に供給される。なお、以下においては、力覚センサ５１を適宜、「力測定手段５１」と呼ぶ場合がある。
【００２６】
また、力覚センサ５１を用いることなしに、ロボットアーム５０ａを駆動するアクチュエータ（図示しない）の電流値に基づいて力を推定するようにしてもよい。そのような場合には、電流値に基づいて力を推定する手段が力測定手段５１に相当するものとする。
【００２７】
図２は本発明に基づくロボット制御装置の機能ブロック図である。マイクロコンピュータであるロボット制御装置１０は、第一パラメータ格納部２０と第二パラメータ格納部２５とを含んでいる。図示されるように、第一パラメータ格納部２０には、予め設定された設定値である、目標押付力・目標モーメントＦｓ、目標進行方向速度Ｖｓ、目標軌道Ｔｓおよび制御ゲイン、例えば力制御ゲインＧｓが格納されている。また、第二パラメータ格納部２５には、作業ツール５２の質量・重心位置２６が格納されている。さらに、算出方法を記載しない後述する定数もこれらパラメータ格納部に予め格納されているものとする。
【００２８】
ここで、目標軌道Ｔｓは、ＣＡＤデータによって得られる軌道、加工作業前に作業ツール５２をワーク６０の表面上に実際に倣わせることにより得られる軌道、教示作業によって得られる軌道などを含むものとする。
【００２９】
さらに、ロボット制御装置１０は、種々のロボット操作指令を作成・演算してロボット５０に供給する指令演算部１１と、第一パラメータ格納部２０に格納されたパラメータの値に基づいて、重力および動力学項が力測定手段５１に及ぼす力に応じて、目標押付力、目標進行方向速度、目標軌道、制御ゲインなどを調整して、それらの値を指令演算部１１に送る指令調整手段１２と、ロボット５０に取付けられた位置、速度、加速度検出部５３から送られるデータと、第二パラメータ格納部に予め格納されたツールの質量、重心位置２６とに基づいて動力学項および重力を算出する動力学項・重力算出部１３（以下、「算出部１３」と称す。）と、ロボット５０の力測定手段５１、例えば力覚センサにより測定された力の出力値を、力覚センサ座標系から見た値に変換すると共に、重力および動力学項が及ぼす影響を補正し、作業ツール５２とワーク６０の間に作用する正味の力を算出する力補正手段１４とを備えている。
【００３０】
図３はロボット制御装置により制御されるロボットアーム先端部分の移動を説明するための図である。図３においては、ワーク６０の表面６２に沿って、ロボット５０の作業ツール５２が倣い加工作業する様子が示されている。図３に示されるようにワーク６０の表面６２は大きな凹凸を含んでいる。このため、作業ツール５２の移動方向は、表面６２の領域６３および領域６４において、比較的大きく変化するようになる。ここで、領域６３においては慣性力の押付方向成分（遠心力）の作用する方向と押付方向とが逆向きであり、領域６４においては慣性力の押付方向成分（遠心力）の作用する方向と押付方向とが同一の向きである。なお、図３においては、作業ツール５２の進行方向はワーク６０の表面６２に沿った方向であり、作業ツール５２がワーク６０を押付ける押付方向は作業ツール５２が対応するワーク６０の表面６２に対して垂直でかつワーク６０内部に向かう方向であるものとする。
【００３１】
以下、図２および図３を参照しつつ、本発明のロボット制御装置１０による制御について説明する。本発明においては、重力が力測定手段５１に作用する力と、ロボットアーム５０ａを駆動することによってエンドエフェクタ、例えば作業ツール５２が力測定手段５１に作用する力である動力学項とを算出する。
【００３２】
これらの力は、以下に表すマニピュレータの動力学方程式を解く際に用いられる公知の方法を利用することによって、算出することができる。
【数１】
2008142810.tif 000003
ここで、τは関節トルクベクトルであり、θは関節角度ベクトルであり、Ｍ（θ）は質量行列であり、Ｃ（θ、ｄθ／ｄｔ）は遠心力やコリオリ力などを表すベクトルであり、Ｇ（θ）は重力ベクトルである。
【００３３】
前記動力学方程式を解く際に一般的に使用されている方法、例えばニュートン・オイラー法やラグランジュ法などを用いて、ロボットアームの各リンクの重心に作用する重力および慣性力（慣性モーメントも含む）を算出できる。この方法を用いて、ロボットアームの関節角度ベクトルθ、該関節角度ベクトルθの速度および加速度から、力覚センサの座標系から見た、作業ツール５２の重心に作用する慣性力が力覚センサの座標系中心に作用する力を求めることによって、動力学項を算出することができる。また、力覚センサの座標系から見た、作業ツール５２の重量が力覚センサの座標系中心に作用する力も同時に算出することができる。（ここで、ロボットアームの角速度を関節角度ベクトルから算出してもよく、角加速度を関節角度ベクトルや角速度から算出してもよい。）
【００３４】
ロボット制御装置１０の算出部１３は、重力が力覚センサ５１の座標系中心に作用する力と、ロボットアームを動かすことによってエンドエフェクタが力覚センサ５１の座標系中心に作用する力である動力学項とを上記のように算出する。
【００３５】
一方、ロボットアーム５０ａが図３を参照して説明したように移動するときに、力覚センサ５１は力を測定する。次いで、ロボット制御装置１０の力補正手段１４は、測定された力から動力学項および重力の及ぼす影響を補正する。具体的には、重力が力覚センサ５１の座標系中心に作用する力と前述した動力学項とを測定された力から減算する。これにより、ロボットアーム５０ａがその先端部分の移動方向が比較的大きく変化する動作を行うときに、作業ツール５２とワーク６０との間に作用する正味の力の推定誤差を小さくすることが可能となる。このため、本発明においては、作業ツール５２とワーク６０との間に作用する押付力をより正確に制御することが可能となる。
【００３６】
なお、作業ツール用の位置・速度・加速度検出部（図示しない）を作業ツール５２に取付け、これによって検出される作業ツール５２の加速度を用いて、動力学項および重力を算出するようにしてもよい。
【００３７】
その後、ロボット制御装置１０の指令調整手段１２は、重力および動力学項の合力、または動力学項のみを所定の閾値と比較することによって、ロボットアームの動作指令に関連するパラメータの値を調整する。この閾値は、以下の式（２）のように目標押付力Ｆｓから算出される。
ｈｔ＝ｋ０・Ｆｓ                                                        （２）
ここで、ｈｔは重力および動力学項の合力、または動力学項のみと比較するための閾値であり、ｋ０は定数であり、Ｆｓは予め設定した目標押付力である。このように閾値ｈｔを、予め設定した目標押付力Ｆｓに基づいて定める場合には、作業ツール５２、後述するハンド５４またはワーク６０が取替えられた場合であっても同一の調整値を使用することが可能となる。
【００３８】
以下では、動力学項として慣性力の押付方向成分を考え、指令調整手段１２は押付方向成分の値と上記閾値とを比較してロボットアームの動作指令に関連するパラメータの値の調整を行う。（ここで、動力学項における押付方向以外の力を用いた調整を行ってもよい。）
【００３９】
以下、動作指令に関連のあるパラメータの値の調整として、
（１）目標押付力および目標進行方向速度の両方を変更する場合、
（２）目標進行方向速度のみを変更する場合、
（３）目標押付力のみを変更する場合、
（４）目標軌道を調整する場合、
（５）力制御ゲインを調整する場合、について説明する。
【００４０】
（１）目標押付力および目標進行方向速度の両方を変更する場合
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより小さい場合には、目標進行方向速度Ｖｄはその設定値Ｖｓを使用する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、目標進行方向速度Ｖｄを小さくすると共に、目標押付力Ｆｄも小さくする（１−１）、（１−２）。これにより、目標進行方向速度Ｖｄを小さくしてもワーク６０が過剰に加工されるのを防止できる。（ただし、ワーク６０と作業ツール５２とが離れやすくなるような部分において目標進行方向速度Ｖｄをそれほど低下させたくない場合には、目標押付力Ｆｄを大きくする。）
【００４１】
目標進行方向速度Ｖｄおよび目標押付力Ｆｄを小さくするために、以下の二つの手法を採用できる。
（１−１）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、閾値を超えてから経過した時間ｔに基づいて目標進行方向速度Ｖｄと目標押付力Ｆｄとを変更する。
図５は目標進行方向速度Ｖｄと閾値を超えてからの経過時間ｔとの関係を示す図である。図５においては、慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔを超えた時点における速度をＶｘとする。（ここで、Ｖｘは、｜ｈ｜が閾値ｈｔを超えた後に下回り、また超えると、新たな値に更新される。）
【００４２】
目標進行方向速度Ｖｄを以下の式（３）に基づいて滑らかに減速させつつ、式（４）に基づいて、目標押付力Ｆｄを小さく変更する。
Ｖｄ＝｛（１−ｋ１１）・ｅｘｐ（−ｌ１１・ｔ）＋ｋ１１｝・Ｖｘ          （３）
ただし、０＜ｌ１１、０＜ｔ、０≦ｋ１１＜１、０＜ｈｔ＜｜ｈ｜
Ｆｄ＝ｍ１１・（Ｆｓ／Ｖｓ）・Ｖｄ＋ｎ１１                              （４）
ただし、０＜ｍ１１
【００４３】
式（３）および式（４）においては、Ｖｄは目標進行方向速度であり、ｋ１１は定数であり、ｌ１１は定数であり、ｔは慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔを超えてから経過した時間を示している。さらに、Ｖｘは慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔを超えた時点での速度であり、Ｆｄは目標押付力であり、ｍ１１は定数であり、Ｆｓは予め設定した目標押付力であり、Ｖｓは予め設定した目標進行方向速度である。さらに、ｎ１１は定数であり、ｈは慣性力の押付方向成分であり、ｈｔは動力学項と比較するための閾値である。
【００４４】
（１−２）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて目標進行方向速度Ｖｄと目標押付力Ｆｄとを変更する。
図６は係数α１、α２と慣性力の押付方向成分の大きさとの関係を示す図である。はじめに、以下の式（５）、（６）により表されるように、慣性力の押付方向成分の大きさ｜ｈ｜に応じて変化する係数α１、α２を求める。
α１＝（１−ｋ１２）・ｅｘｐ｛−ｌ１２・（｜ｈ｜−ｈｔ）｝＋ｋ１２      （５）
ただし、０＜ｌ１２、０≦ｋ１２＜１、０＜ｈｔ＜｜ｈ｜
α２＝（１−ｍ１２）・ｅｘｐ｛−ｎ１２・（｜ｈ｜−ｈｔ）｝＋ｍ１２      （６）
ただし、０＜ｎ１２、０≦ｍ１２＜１、０＜ｈｔ＜｜ｈ｜
【００４５】
次いで、これら係数α１、α２を用いて、目標進行方向速度Ｖｄおよび目標押付力Ｆｄを以下の式（７）、（８）に基づいて小さくする。
Ｖｄ＝α１・Ｖｓ                                                        （７）
Ｆｄ＝α２・Ｆｓ                                                        （８）
【００４６】
式（５）および式（６）においては、ｋ１２は定数であり、ｌ１２は定数であり、ｍ１２は定数であり、ｎ１２は定数である。なお、目標進行方向速度、目標押付力を変化させる場合には、ロボットアームの動きに過大な負荷がかからないように、滑らかに変化させるのが好ましい。
【００４７】
（２）目標進行方向速度のみを変化させる場合
以下のように、作業ツール５２とワーク６０との相対速度を調節することにより、加工が不十分または過剰になるのを防止する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより小さい場合には、目標進行方向速度Ｖｄはその設定値Ｖｓを使用する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、目標進行方向速度Ｖｄを小さくする（２−１）、（２−２）。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向とが同じ向きである場合には、目標進行方向速度Ｖｄを大きくする（２−３）、（２−４）。なお、目標進行方向速度Ｖｄを大きくする理由は、目標進行方向速度Ｖｄの増加を、押付力が大きくなることによる単位時間当たりの切削量の増加と相殺するためである。
【００４８】
これらについて、以下に説明する。
（２−１）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、閾値を超えてから経過した時間ｔに基づいて目標進行方向速度Ｖｄを小さくする。このときには、前述した式（３）を使用して、目標進行方向速度Ｖｄを小さくする。
【００４９】
（２−２）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて目標進行方向速度Ｖｄを小さくする。このときには、前述した式（５）から係数α１を算出した後で、前述した式（７）を用いて目標進行方向速度Ｖｄを小さくする。
【００５０】
次いで、慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向とが同じ向きである場合について説明する。
（２−３）、（２−４）に関して目標進行方向速度Ｖｄを大きくすることを記述したが、目標進行方向速度Ｖｄを大きくする場合には慣性力の押付方向成分の大きさ｜ｈ｜をさらに大きくする副作用が生じるので制御性能が悪化する場合がある。そのような場合には、（２−１）、（２−２）と同様に目標進行方向速度Ｖｄを小さくするのが好ましい。このことは、単位時間当たりの加工量が小さくて進行方向速度の影響を受けにくい、バフ掛けなどの研磨作業の場合に適用できる。
一方、単位時間当たりの切削量が大きい用途、例えばバリ取り作業などにおいては、目標進行方向速度Ｖｄを小さくして押付時間が長くなると加工が過剰になる不具合が発生する。
従って、バリ取り作業などでは、以下の（２−３）、（２−４）のように目標進行方向速度Ｖｄを大きくする。しかしながら、制御性能が著しく悪化する場合には、（１−１）、（１−２）のように目標進行方向速度Ｖｄと目標押付力Ｆｄとを変更するのが好ましい。
【００５１】
（２−３）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである場合には、閾値を超えてから経過した時間に基づいて目標進行方向速度Ｖｄを大きくする。
【００５２】
図７は目標進行方向速度Ｖｄと閾値を超えてからの経過時間ｔとの関係を示す他の図である。図７においては、慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔを超えた時点での速度をＶｘとする（ここで、Ｖｘは、｜ｈ｜が閾値ｈｔを超えた後に下回り、また超えると、新たな値に更新される）。
【００５３】
図７を参照しつつ、以下の式（９）に基づいて目標進行方向速度Ｖｄを算出する。
Ｖｄ＝｛（ｋ２３−１）・（１−ｅｘｐ（−ｌ２３・ｔ））＋１｝・Ｖｘ      （９）
ただし、０＜ｌ２３、０＜ｔ、１≦ｋ２３、０＜ｈｔ＜｜ｈ｜
ここで、ｋ２３は定数であり、ｌ２３は定数である。
【００５４】
（２−４）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて目標進行方向速度Ｖｄを大きくする。
【００５５】
図８は係数β１と慣性力の押付方向成分の大きさ｜ｈ｜との関係を示す図である。はじめに、以下の式（１０）に基づいて、慣性力の押付方向成分の大きさ｜ｈ｜に応じて変化するような係数β１を算出する。
β１＝（ｋ２４−１）・（１−ｅｘｐ｛−ｌ２４・（｜ｈ｜−ｈｔ）｝）＋１  （１０）
ただし、０＜ｌ２４、１≦ｋ２４、０＜ｈｔ＜｜ｈ｜
【００５６】
次いで、この係数β１に基づいて、以下の式（１１）から目標進行方向速度Ｖｄを算出する。
Ｖｄ＝β１・Ｖｓ                                                      （１１）
ここで、ｋ２４は定数であり、ｌ２４は定数である。なお、目標進行方向速度を変化させる場合には、ロボットアームの動きに過大な負荷がかからないように、滑らかに変化させるのが好ましい。
【００５７】
（３）目標押付力のみを変化させる場合
以下のように、目標押付力Ｆｄを調節することにより、加工が不十分または過剰になるのを防止する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより小さい場合には、目標押付力Ｆｄはその設定値Ｆｓを使用する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、目標押付力Ｆｄを大きくする。（３−１）、（３−２）
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである場合には、目標押付力Ｆｄを小さくする。（３−３）、（３−４）
【００５８】
（３−１）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、閾値を超えてから経過した時間ｔに基づいて目標押付力Ｆｄを大きくする。
【００５９】
図９は目標押付力Ｆｄと閾値を超えてからの経過時間ｔとの関係を示す図である。図９を参照しつつ、以下の式（１２）に基づいて目標押付力Ｆｄを算出する。
Ｆｄ＝｛（ｋ３１−１）・（１−ｅｘｐ（−ｌ３１・ｔ））＋１｝・Ｆｓ    （１２）
ただし、０＜ｌ３１、０＜ｔ、１≦ｋ３１、０＜ｈｔ＜｜ｈ｜
ここで、ｋ３１は定数であり、ｌ３１は定数である。
【００６０】
（３−２）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が逆向きである場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて目標押付力Ｆｄを大きくする。このときには、前述した式（１０）から係数β１を算出した後で、以下の式（１３）を用いて目標押付力Ｆｄを大きくする。
Ｆｄ＝β１・Ｆｓ                                                      （１３）
【００６１】
（３−３）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである場合には、閾値を超えてから経過した時間に基づいて目標押付力Ｆｄを小さくする。
【００６２】
図１０は目標押付力Ｆｄと閾値を超えてからの経過時間ｔとの関係を示す他の図である。図１０を参照しつつ、以下の式（１４）に基づいて、目標押付力Ｆｄを算出する。
Ｆｄ＝｛（１−ｋ３３）・ｅｘｐ（−ｌ３３・ｔ）＋ｋ３３｝・Ｆｓ        （１４）
ただし、０＜ｌ３３、０＜ｔ、０≦ｋ３３＜１、０＜ｈｔ＜｜ｈ｜
ここで、ｋ３３は定数であり、ｌ３３は定数である。
【００６３】
（３−４）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きく、かつ、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて目標押付力Ｆｄを小さくする。このときには、前述した式（５）から係数α１を算出した後で、前述した式（１３）を用いて目標押付力Ｆｄを小さくする。なお、目標押付力を変化させる場合には、ロボットアームの動きに過大な負荷がかからないように、滑らかに変化させるのが好ましい。
【００６４】
なお、目標押付力Ｆｄおよび目標進行方向速度Ｖｄを小さくする場合にはワークが加工不足となることまたは過剰に加工されるのを防止でき、目標押付力Ｆｄを大きくおよび／または目標進行方向速度Ｖｄを小さくする場合にはワークが加工不足となるのを防止でき、目標押付力Ｆｄを小さくおよび／または目標進行方向速度Ｖｄを大きくする場合にはワークが過剰に加工されるのを防止できるのは、当業者であれば明らかであろう。
【００６５】
ところで、ロボット制御装置１０の指令調整手段１２が目標軌道Ｔｄの調整を行うようにしてもよい。以下では、動力学項として慣性力の押付方向成分を考え、その値と前述した閾値を比較して目標軌道Ｔｄの調整を行う（ここで、動力学項における押付方向以外の力を用いた調整を行ってもよい）。
【００６６】
（４）目標軌道を調整する場合
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより小さい場合には、目標軌道Ｔｄはその設定値Ｔｓを使用する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、慣性力の押付方向成分の作用する方向と押付方向に応じて、以下のように目標軌道をずらす。
第一には、慣性力の押付方向成分の作用する方向と押付方向とが逆向きである場合には、押付方向に目標軌道をずらす。
第二には、慣性力の押付方向成分の作用する方向と押付方向とが同じ向きである場合には、押付方向とは逆向きに目標軌道をずらす。
【００６７】
目標軌道Ｔｄは、以下の式によって与えられるようにする。
Ｔｄ＝Ｔｓ＋Ｔｍ
目標軌道の修正量Ｔｍは以下の二通りで算出される。（４−１）、（４−２）
【００６８】
（４−１）目標軌道の修正量Ｔｍを、閾値を超えてから経過した時間ｔに基づいて算出する。図１１は軌道修正量Ｔｍと閾値を超えてからの経過時間ｔとの関係を示す図である。図１１を参照しつつ、目標軌道の修正量Ｔｍを以下の式（１５）から算出する。
Ｔｍ＝ｋ４１・（１−ｅｘｐ（−ｌ４１・ｔ））                          （１５）
ただし、０＜ｌ４１、０＜ｔ、０≦ｋ４１、０＜ｈｔ＜｜ｈ｜
ここで、ｋ４１は定数であり、ｌ４１は定数である。
【００６９】
（４−２）目標軌道の修正量Ｔｍを、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて算出する。図１２は軌道修正量Ｔｍと慣性力の押付方向成分の大きさ｜ｈ｜との関係を示す他の図である。図１２を参照しつつ、目標軌道の修正量Ｔｍを以下の式（１６）から算出する。
Ｔｍ＝ｋ４２・（１−ｅｘｐ｛−ｌ４２・（｜ｈ｜−ｈｔ）｝）            （１６）
ただし、０＜ｌ４２、０≦ｋ４２、０＜ｈｔ＜｜ｈ｜
ここで、ｋ４２は定数であり、ｌ４２は定数である。さらに、Ｔｍは軌道修正量であり、ｈは慣性力の押付方向成分であり、ｈｔは動力学項と比較するための閾値である。これにより、ワークが加工不足または加工過剰になるのを防止できるのが分かるであろう。なお、目標軌道を変化させる場合には、ロボットアームの動きに過大な負荷がかからないように、滑らかに変化させるのが好ましい。
【００７０】
ところで、倣い加工作業において力測定手段５１によって得られる力は、ワーク６０と作業ツール５２とが接触しているためにワーク６０の表面状態、バリの大きさ、ロボットアーム５０ａ自体の動きによる撓みまたは揺れなどの影響を受けやすい。また、力測定手段５１の信号にのるノイズ、作業ツール５２の回転による力または作業ツール５２の振動によるノイズなどの外乱も存在する。このため、力制御ゲインを過度に大きな値に設定すると、外乱に対して過度に反応したりロボットアーム５０ａが発振しやすくなる。そのため、必要以上にゲインを大きく設定しないことが望ましい。
【００７１】
このため、通常は制御ゲインの大きさを比較的小さくしておき、慣性力が比較的大きい場合にのみ制御ゲインを比較的大きくするのが望まれる。この場合、動力学項の値に応じて、ロボットアーム５０ａの動作に関する指令を調整する指令調整手段１２が力制御ゲインを調整するようにしてもよい。これにより、制御ゲインを必要な領域においてのみ大きくし、通常は比較的小さくしておくことができる。従って、ワーク６０の加工状態を所望の状態にすることができる。
【００７２】
以下では、動力学項として慣性力の押付方向成分を考え、その値と前述した閾値とを比較して力制御ゲインの調整を行う（ここで、動力学項における押付方向以外の力を用いた調整を行ってもよい）。なお、ロボット制御装置１０により実施される力制御はインピーダンス制御またはダンピング制御のいずれであってもよい。
【００７３】
（５）力制御ゲインを調整する場合
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより小さい場合には、力制御ゲインＧｄはその設定値Ｇｓを使用する。
・慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、力制御ゲインＧｄを大きくする（５−１）、（５−２）。
【００７４】
（５−１）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、閾値を超えてから経過した時間ｔに基づいて力制御ゲインＧｄを大きくする。図１３は力制御ゲインＧｄと閾値を超えてからの経過時間ｔとの関係を示す図である。図１３を参照しつつ、以下の式（１７）から力制御ゲインＧｄを算出する。
Ｇｄ＝｛（ｋ５１−１）・（１−ｅｘｐ（−ｌ５１・ｔ））＋１｝・Ｇｓ    （１７）
ただし、０＜ｌ５１、０＜ｔ、１≦ｋ５１、０＜ｈｔ＜｜ｈ｜
ここで、ｋ５１は定数であり、ｌ５１は定数である。
【００７５】
（５−２）慣性力の押付方向成分の大きさ｜ｈ｜が閾値ｈｔより大きい場合には、慣性力の押付方向成分の大きさ｜ｈ｜に基づいて力制御ゲインＧｄを大きくする。このときには、前述した式（１０）から係数β１を算出した後で、以下の式（１８）を用いて力制御ゲインＧｄを算出する。
Ｇｄ＝β１・Ｇｓ                                                      （１８）
ここで、Ｇｓは予め第一パラメータ格納部２０に設定した力制御ゲインである。
また、上記(1)、(2)、(3)、(4)、(5)の場合において、目標進行方向速度Ｖｄ、目標押付力Ｆｄ、目標軌道修正量Ｔｍ、力制御ゲインＧｄを、慣性力の押付方向成分の大きさとその方向に応じて設定するようなテーブルを予め用意し、それを利用する方法をおこなってもよい。
【００７６】
このように本発明においては、ロボットアームの動作に関連する指令を調整する際に、加工経路上の調整箇所を予め教示する必要はなく、また加工経路上の曲率を別途算出する必要もない。また、重力および動力学項から、加工が不十分または過剰になりそうであるかを判断し、それに応じて動作指令を調節している。従って、ワークを容易かつ正確に加工できるのが分かるであろう。
【００７７】
ところで、図４は本発明に基づくロボット制御装置により制御される他のロボットの斜視図である。図４に示されるロボット５０’はロボットアーム５０ａの先端に、ワーク６０を把持するためのハンド５４が取付けられている。ハンド５４に把持されたワーク６０が床に固定されたグラインダ５２に押付けられつつ、倣い加工作業が行われる。そして、力覚センサ５１はロボットアーム５０ａとハンド５４との間に配置されている。なお、力覚センサ５１を用いることなしに、ロボットアーム５０ａを駆動するアクチュエータの電流値に基づいて力を推定するようにしてもよい。このような場合であっても、前述した制御を適用できるのは、当業者であれば明らかであろう。
【図面の簡単な説明】
【００７８】
【図１】本発明に基づくロボット制御装置により制御されるロボットの斜視図である。
【図２】本発明に基づくロボット制御装置の機能ブロック図である。
【図３】ロボット制御装置により制御されるロボットアーム先端部分の移動を説明するための図である。
【図４】本発明に基づくロボット制御装置により制御される他のロボットの斜視図である。
【図５】目標進行方向速度Ｖｄと閾値を超えてからの経過時間ｔとの関係を示す図である。
【図６】係数α１、α２と慣性力の押付方向成分の大きさとの関係を示す図である。
【図７】目標進行方向速度Ｖｄと閾値を超えてからの経過時間ｔとの関係を示す他の図である。
【図８】係数β１と慣性力の押付方向成分の大きさ｜ｈ｜との関係を示す図である。
【図９】目標押付力Ｆｄと閾値を超えてからの経過時間ｔとの関係を示す図である。
【図１０】目標押付力Ｆｄと閾値を超えてからの経過時間ｔとの関係を示す他の図である。
【図１１】軌道修正量Ｔｍと閾値を超えてからの経過時間ｔとの関係を示す図である。
【図１２】軌道修正量Ｔｍと慣性力の押付方向成分の大きさ｜ｈ｜との関係を示す他の図である。
【図１３】力制御ゲインＧｄと閾値を超えてからの経過時間ｔとの関係を示す図である。
【符号の説明】
【００７９】
１０    ロボット制御装置
１１    指令演算部
１２    指令調整手段
１３    動力学項、重力算出部
１４    力補正手段
２０    第一パラメータ格納部
２５    第二パラメータ格納部
２６    重心位置
５０、５０’    ロボット
５０ａ    ロボットアーム
５１    力覚センサ（力測定手段）
５２    作業ツール
５３    位置・速度・加速度検出部
５４    ハンド
６０    ワーク
６１    作業台
６２    表面
６３    ワーク表面において、慣性力の押付方向成分の作用する方向と押付方向が逆向きである領域
６４    ワーク表面において、慣性力の押付方向成分の作用する方向と押付方向が同じ向きである領域
Ｔｄ    目標軌道
Ｔｓ    目標軌道の設定値
Ｔｍ    軌道修正量
Ｆｄ    目標押付力
Ｆｓ    目標押付力の設定値
Ｇ（θ）    重力ベクトル
Ｇｄ    力制御ゲイン
Ｇｓ    力制御ゲインの設定値
Ｃ（θ，ｄθ／ｄｔ）    遠心力やコリオリ力などを表すベクトル
ｈ    慣性力の押付方向成分
ｈｔ    閾値
ｔ    閾値を超えてからの経過時間
Ｖｄ    目標進行方向速度
Ｖｓ    目標進行方向速度の設定値
α１、α２、β１    係数
【図１】
2008142810.tif 000004
【図２】
2008142810.tif 000005
【図３】
2008142810.tif 000006
【図４】
2008142810.tif 000007
【図５】
2008142810.tif 000008
【図６】
2008142810.tif 000009
【図７】
2008142810.tif 000010
【図８】
2008142810.tif 000011
【図９】
2008142810.tif 000012
【図１０】
2008142810.tif 000013
【図１１】
2008142810.tif 000014
【図１２】
2008142810.tif 000015
【図１３】
2008142810.tif 000016