JP7194910B2 - ロボット制御方法及びロボット制御装置 - Google Patents

Description

本発明は、ロボット制御方法及びロボット制御装置に関するものである。

従来より、モータの回転方向を反転させたときに、制御数値の目標移動量と、テーブルやワークの相対的な移動量とが一致しなくなる、いわゆるバックラッシュ誤差が生じることが知られている（例えば、特許文献１参照）。

特許文献１には、位置指令信号の増減方向反転後の経過時間に応じたレベル割合において補正後のステップ信号の大きさを削減し、これを位置補正信号（バックラッシュ情報）として出力することで、バックラッシュ補正を行うようにした構成が開示されている。

特許第３７０３６６４号公報

ここで、本願発明者らは、ロボットアームの位置ずれ、つまり、軌跡誤差が生じる要因として、バックラッシュ誤差の他にも、モータの回転速度が上昇すると、モータを反転させたときの反転時トルクが大きくなり、モータの減速機に瞬間的なねじれが加わって軌跡誤差が生じることを見出した。

しかしながら、従来のバックラッシュ補正装置は、バックラッシュ誤差のみを補正するものであり、反転時トルクによるねじれの影響については何ら考慮されていないことから、ロボットアームの軌跡誤差が大きくなってしまうという問題がある。

本発明は、かかる点に鑑みてなされたものであり、その目的は、ロボットアームの軌跡誤差を低減することにある。

本開示の態様は、複数の関節部を有するロボットアームの動作を制御するためのロボット制御方法を対象とし、次のような解決手段を講じた。

すなわち、第１の態様は、前記複数の関節部のうち、補正対象の該関節部の角速度を演算するステップと、
前記複数の関節部の位置指令に基づいて、前記補正対象の関節部にかかるトルクを演算するステップと、
前記補正対象の関節部の角速度に基づいて、該関節部の回転方向が反転する反転タイミングを演算するステップと、
前記トルクと前記反転タイミングとに基づいて、前記補正対象の関節部にかかる反転時トルクを演算するステップと、
前記反転時トルクと前記反転タイミングとに基づいて、該反転時トルクによって該補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を演算するステップとを備えたことを特徴とするものである。

第１の態様では、補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を、反転時トルクと反転タイミングとに基づいて演算するようにしている。

このように、補正対象の関節部で生じるねじれの影響を考慮した制御を行うことで、ロボットアームの軌跡誤差を低減することができる。

第２の態様は、第１の態様において、
前記反転時トルクが所定の閾値よりも小さい場合には、前記補正対象の関節部で生じるバックラッシュを補正するためのバックラッシュ補正量に基づいて該関節部の位置指令を補正する一方、該反転時トルクが所定の閾値よりも大きい場合には、該バックラッシュ補正量と前記ねじれ補正量及び補正時間に基づいて該関節部の位置指令を補正するステップを備えたことを特徴とするものである。

第２の態様では、反転時トルクが所定の閾値よりも小さい場合には、補正対象の関節部で生じるねじれの影響が小さいと判断して、関節部で生じるバックラッシュのみを補正するようにしている。そして、反転時トルクが所定の閾値よりも大きい場合には、補正対象の関節部で生じるバックラッシュに加えて、ねじれについても補正するようにしている。

第３の態様は、第１又は第２の態様において、
前記ねじれ補正量及び補正時間を演算するステップでは、前記反転時トルクが大きいほど、該ねじれ補正量が大きく且つ該補正時間が短くなるように、該ねじれ補正量及び該補正時間が演算されることを特徴とするものである。

第３の態様では、反転時トルクが大きいほど、ねじれ補正量を大きく且つ補正時間を短くするようにしている。これにより、関節部のねじれを効果的に低減して、ロボットアームの位置ずれを精度良く補正することができる。

第４の態様は、複数の関節部を有するロボットアームの動作を制御するためのロボット制御方法を対象としている。そして、前記複数の関節部のうち補正対象の該関節部は、前記ロボットアームを旋回させる旋回軸であり、
前記旋回軸の位置指令に基づいて、該旋回軸の角速度を演算するステップと、
前記旋回軸の角速度に基づいて、該旋回軸の加速度を演算するステップと、
前記旋回軸の角速度に基づいて、該旋回軸の回転方向が反転する反転タイミングを演算するステップと、
前記旋回軸の加速度と前記反転タイミングとに基づいて、該旋回軸の反転時加速度を演算するステップと、
前記反転時加速度と前記反転タイミングとに基づいて、前記旋回軸の反転時に該旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を演算するステップとを備えたことを特徴とするものである。

第４の態様では、補正対象の関節部である旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を、反転時加速度と反転タイミングとに基づいて演算するようにしている。

このように、旋回軸で生じるねじれの影響を考慮した制御を行うことで、ロボットアームの軌跡誤差を低減することができる。

また、ねじれ補正量及び補正時間を、反転時加速度に基づいて演算しているので、例えば、動力学トルクに基づいてねじれ補正量及び補正時間を演算する場合に比べて、演算量を減らすことができる。

具体的に、動力学トルクは、関節部の軸に印加される全てのトルク（重力トルク、慣性力、遠心力、コリオリ力等）を含めて演算する必要がある。そのため、全軸の位置、速度、加速度、機械パラメータを用いて逆動力学演算を実行する必要があり、動力学トルクを正確に演算することができる反面、演算処理が重くなってしまう。

ここで、本願発明者は、重力が印加されない旋回軸については、動力学トルクの主要成分が自軸の慣性力となることに着目した。そして、旋回軸のみについて、反転時加速度に基づいてねじれ補正量及び補正時間を演算するようにしたから、演算量を減らすことができる。

第５の態様は、第４の態様において、
前記反転時加速度が所定の閾値よりも小さい場合には、該旋回軸で生じるバックラッシュを補正するためのバックラッシュ補正量に基づいて該旋回軸の位置指令を補正する一方、該反転時加速度が所定の閾値よりも大きい場合には、該バックラッシュ補正量と前記ねじれ補正量及び補正時間に基づいて該旋回軸の位置指令を補正するステップを備えたことを特徴とするものである。

第５の態様では、反転時加速度が所定の閾値よりも小さい場合には、旋回軸で生じるねじれの影響が小さいと判断して、旋回軸で生じるバックラッシュのみを補正するようにしている。そして、反転時加速度が所定の閾値よりも大きい場合には、旋回軸で生じるバックラッシュに加えて、ねじれについても補正するようにしている。

第６の態様は、第４又は第５の態様において、
前記ねじれ補正量及び補正時間を演算するステップでは、前記反転時加速度が大きいほど、該ねじれ補正量が大きく且つ該補正時間が短くなるように、該ねじれ補正量及び該補正時間が演算されることを特徴とするものである。

第６の態様では、反転時加速度が大きいほど、ねじれ補正量を大きく且つ補正時間を短くするようにしている。これにより、旋回軸のねじれを効果的に低減して、ロボットアームの位置ずれを精度良く補正することができる。

第７の態様は、複数の関節部を有するロボットアームの動作を制御するためのロボット制御装置を対象としている。そして、前記複数の関節部のうち、補正対象の該関節部の角速度を演算する角速度演算部と、
前記複数の関節部の位置指令に基づいて、前記補正対象の関節部にかかるトルクを演算する動力学演算部と、
前記補正対象の関節部の角速度に基づいて、該関節部の回転方向が反転する反転タイミングを演算する反転タイミング演算部と、
前記トルクと前記反転タイミングとに基づいて、前記補正対象の関節部にかかる反転時トルクを演算する反転時トルク演算部と、
前記反転時トルクと前記反転タイミングとに基づいて、該反転時トルクによって該補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を演算する補正量演算部とを備えたことを特徴とするものである。

第７の態様では、補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を、反転時トルクと反転タイミングとに基づいて演算するようにしている。

このように、補正対象の関節部で生じるねじれの影響を考慮した制御を行うことで、ロボットアームの軌跡誤差を低減することができる。

第８の態様は、複数の関節部を有するロボットアームの動作を制御するためのロボット制御装置を対象としている。そして、前記複数の関節部のうち補正対象の該関節部は、前記ロボットアームを旋回させる旋回軸であり、
前記旋回軸の位置指令に基づいて、該旋回軸の角速度を演算する角速度演算部と、
前記旋回軸の角速度に基づいて、該旋回軸の加速度を演算する加速度演算部と、
前記旋回軸の角速度に基づいて、該旋回軸の回転方向が反転する反転タイミングを演算する反転タイミング演算部と、
前記旋回軸の加速度と前記反転タイミングとに基づいて、該旋回軸の反転時加速度を演算する反転時加速度演算部と、
前記反転時加速度と前記反転タイミングとに基づいて、前記旋回軸の反転時に該旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を演算する補正量演算部とを備えたことを特徴とするものである。

第８の態様では、補正対象の関節部である旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を、反転時加速度と反転タイミングとに基づいて演算するようにしている。

このように、旋回軸で生じるねじれの影響を考慮した制御を行うことで、ロボットアームの軌跡誤差を低減することができる。

また、ねじれ補正量及び補正時間を、反転時加速度に基づいて演算しているので、例えば、動力学トルクに基づいてねじれ補正量を演算する場合に比べて、演算量を減らすことができる。

本開示の態様によれば、ロボットアームの軌跡誤差を低減することができる。

図１は、本実施形態１に係るロボットの構成を示す図である。 図２は、反転時トルクが所定の閾値よりも小さいときの軌跡ずれを示す図である。 図３は、軌跡ずれをバックラッシュ補正量のみで補正するときの図である。 図４は、補正後の実軌跡を示す図である。 図５Ａは、補正前の軌跡ずれを示す図である。 図５Ｂは、補正後の実軌跡を示す図である。 図６Ａは、指令速度成分の変化を示すグラフ図である。 図６Ｂは、動力学トルクの変化を示すグラフ図である。 図６Ｃは、補正量の変化を示すグラフ図である。 図７は、反転時トルクが所定の閾値よりも大きいときの軌跡ずれを示す図である。 図８は、軌跡ずれをバックラッシュ補正量のみで補正するときの図である。 図９は、補正後の実軌跡を示す図である。 図１０Ａは、補正前の軌跡ずれを示す図である。 図１０Ｂは、補正後の実軌跡を示す図である。 図１１Ａは、指令速度成分の変化を示すグラフ図である。 図１１Ｂは、動力学トルクの変化を示すグラフ図である。 図１１Ｃは、補正量の変化を示すグラフ図である。 図１２は、ロボットの位置制御に関する機能ブロック図である。 図１３は、ロボットアームにおける減速機のねじれを説明する図である。 図１４は、ロボット制御装置のブロック線図である。 図１５は、バックラッシュ補正ブロックのブロック線図である。 図１６は、反転時トルクが所定の閾値よりも大きいときの軌跡ずれを示す図である。 図１７は、軌跡ずれをバックラッシュ補正量とねじれ補正量とで補正するときの図である。 図１８は、補正後の実軌跡を示す図である。 図１９Ａは、補正前の軌跡ずれを示す図である。 図１９Ｂは、補正後の実軌跡を示す図である。 図２０Ａは、指令速度成分の変化を示すグラフ図である。 図２０Ｂは、動力学トルクの変化を示すグラフ図である。 図２０Ｃは、補正量の変化を示すグラフ図である。 図２１は、反転時トルクとねじれ補正量との関係を示すグラフ図である。 図２２は、反転時トルクと補正時間との関係を示すグラフ図である。 図２３Ａは、指令速度成分の変化を示すグラフ図である。 図２３Ｂは、動力学トルクの変化を示すグラフ図である。 図２３Ｃは、補正量の変化を示すグラフ図である。 図２４は、本実施形態２に係るロボット制御装置のブロック線図である。 図２５は、バックラッシュ補正ブロックのブロック線図である。 図２６Ａは、指令速度成分の変化を示すグラフ図である。 図２６Ｂは、指令加速度成分の変化を示すグラフ図である。 図２６Ｃは、補正量の変化を示すグラフ図である。 図２７は、速度反転時加速度とねじれ補正量との関係を示すグラフ図である。 図２８は、速度反転時加速度と補正時間との関係を示すグラフ図である。

以下、本発明の実施形態を図面に基づいて説明する。なお、以下の好ましい実施形態の説明は、本質的に例示に過ぎず、本発明、その適用物或いはその用途を制限することを意図するものではない。

《実施形態１》
図１に示すように、垂直多関節型のロボット１は、ロボットアーム２と、複数の関節部Ｊ１～Ｊ６とを有している。ロボット１には、ロボット制御装置２０が接続されている。

ロボットアーム２は、複数の部分に分かれており、各部分の連結部に関節部Ｊ１～Ｊ６がそれぞれ設けられている。また、関節部Ｊ１～Ｊ６には、それぞれ減速機１３を介してサーボモータ１２が連結している（図１２参照）。

サーボモータ１２は、ロボット制御装置２０からの位置指令θｃに基づいて駆動して、関節部Ｊ１～Ｊ６をそれぞれ所望の量だけ回転させることで、ロボットアーム２の動作及び姿勢を制御する。

ここで、関節部Ｊ１～Ｊ３は、ロボットアーム２の全体の姿勢を決める主軸３軸であり、関節部Ｊ４～Ｊ６は、ロボットアーム２の先端の方向を決める手首３軸である。関節部Ｊ１は、ロボットアーム２を旋回させる旋回軸である。

以下、ロボットアーム２の先端に図示しないレーザ出力装置を装着し、レーザ光を照射してワークを切断する作業を行う場合について、説明する。

図２に示すように、例えば、直径１０ｍｍの円を０．２ｍ／分で切断する作業を仮定する。図２に示す例では、０°の位置から反時計回り方向に円軌道でレーザ切断を行うものとする。

ここで、関節部Ｊ１のモータ１２の回転方向は、ロボットアーム２の先端が、図２で９０°と２７０°の位置に到達するタイミングでそれぞれ反転する。そのため、９０°と２７０°の位置で、それぞれバックラッシュ誤差が発生する。

具体的に、実軌跡は、図２で反転位置よりも右側の部分、つまり、２７０°から９０°の位置までの半円部分が、直径自体は変化しないで下方向にシフトした軌跡となる。

また、図２で反転位置よりも左側の部分、つまり、９０°から２７０°の位置までの半円部分が、直径自体は変化しないで上方向にシフトした軌跡となる。

このように、実軌跡は、図２で反転位置よりも右側が下方向にシフトし、反転位置よりも左側が上方向にシフトした軌跡となっている。

そこで、図３に示すように、位置指令に対してバックラッシュ補正量を加算又は減算することで、実軌跡が目標軌跡に近付くように補正する。具体的には、０°から９０°位置、及び２７０°から３６０°（０°）位置にかけて、バックラッシュ補正量（以下、「補正量１」とも呼ぶ）を位置指令に加算して、図３で上方向にシフトさせた位置指令に補正する。

一方、９０°から２７０°位置にかけて、補正量１（θＢＬ１）を位置指令から減算して、図３で下方向にシフトさせた位置指令に補正する。

このように、図４に示すような補正加算指令を与えることで、ロボットアーム２の実軌跡が円軌道となるように補正することができる。

以下、図５Ａ及び図５Ｂに示す測定波形を参照しながら、補正前後で軌跡ずれがどの程度変化したかについて説明する。なお、図５Ａ及び図５Ｂに示す測定波形では、誤差形状を明確にするために、誤差を５倍に拡大（デフォルメ）して表示している。図５Ａは、バックラッシュ補正を行わない場合の、目標軌跡と実軌跡との最大誤差を示している。図５Ａに示す例では、最大誤差が０．１１５ｍｍとなっている。

一方、図５Ｂに示すように、バックラッシュ補正を行った場合には、最大誤差が０．０７２ｍｍに低減されていることが分かる。

ところで、直径１０ｍｍの円を０．２ｍ／分で切断する作業では、指令速度成分、動力学トルク、補正量の時間変化は、図６Ａ～図６Ｃに示すグラフ図で表される。

ここで、直径１０ｍｍの円を０．２ｍ／分で切断する条件下では、レーザ切断速度が比較的緩やかであるため、図６Ｂに示すように、反転時トルクτｄｉｒが、所定のトルク閾値τｔｈを越えることはない。

しかしながら、レーザ切断速度が上昇すると、反転時トルクτｄｉｒがトルク閾値τｔｈを越える場合がある。この場合には、バックラッシュ補正を行っただけでは、ロボットアーム２の軌跡誤差を精度良く低減することができない。以下、この点について具体的に説明する。

図７に示すように、例えば、直径１０ｍｍの円を２ｍ／分で切断する作業を仮定する。図７に示す例では、０°の位置から反時計回り方向に円軌道でレーザ切断を行うものとする。

ここで、関節部Ｊ１のモータ１２の回転方向は、ロボットアーム２の先端が、図７で９０°と２７０°の位置に到達するタイミングでそれぞれ反転する。そのため、９０°と２７０°の位置で、それぞれバックラッシュ誤差が発生する。

具体的に、実軌跡は、上述した図２に示す例と同様に、図７で反転位置よりも右側が下方向にシフトし、反転位置よりも左側が上方向にシフトした軌跡となっている。

そして、図７に示す例では、モータ１２の回転速度が速いため、反転時トルクが大きくなって所定のトルク閾値を超えることとなる（図１１Ｂも参照）。そのため、モータ１２の減速機１３にねじれが生じてしまう。そして、ねじれに起因して、反転後の軌跡ずれが、上述した図２に示す例よりも大きくなっている。

特に、主軸である関節部Ｊ１の回転位置は、ロボットアーム２の先端までの距離が長いので、関節部Ｊ１に連結した減速機１３が少しでもねじれると、関節部Ｊ１の回転中心位置が揺動し、ロボットアーム２の先端が描く軌跡に影響を与える。

ここで、上述したのと同様に、０°から９０°位置、及び２７０°から３６０°（０°）位置にかけて、補正量１（θＢＬ１）を位置指令に加算して、図８で上方向にシフトさせた位置指令に補正する。

一方、９０°から２７０°位置にかけて、補正量１（θＢＬ１）を位置指令から減算して、図８で下方向にシフトさせた位置指令に補正する。

しかしながら、図８に示す例では、バックラッシュ誤差に加えて、ねじれ誤差が生じているので、図９に示すようなバックラッシュ補正のみを行うように補正加算指令を与えても、ロボットアーム２の実軌跡が円軌道とはならず、反転後の位置において円軌道の一部が膨らんだ形状となっている。

以下、図１０Ａ及び図１０Ｂに示す測定波形を参照しながら、補正前後で軌跡ずれがどの程度変化したかについて説明する。なお、図１０Ａ及び図１０Ｂに示す測定波形では、誤差形状を明確にするために、誤差を５倍に拡大（デフォルメ）して表示している。図１０Ａは、バックラッシュ補正を行わない場合の、目標軌跡と実軌跡との最大誤差を示している。図１０Ａに示す例では、最大誤差が０．１８３ｍｍとなっている。

一方、図１０Ｂに示すように、バックラッシュ補正を行った場合には、最大誤差が０．１５５ｍｍとなっており、軌跡ずれを十分に低減できていないことが分かる。

ところで、直径１０ｍｍの円を２ｍ／分で切断する作業では、指令速度成分、動力学トルク、補正量の時間変化は、図１１Ａ～図１１Ｃに示すグラフ図で表される。

ここで、直径１０ｍｍの円を２ｍ／分で切断する条件下では、レーザ切断速度が速いため、図１１Ｂに示すように、反転時トルクτｄｉｒが所定のトルク閾値τｔｈを越えている。

そこで、本実施形態では、反転時トルクが所定の閾値よりも大きい場合に、バックラッシュ補正に加えて、ねじれ補正を行うことで、ロボットアーム２の軌跡誤差を低減できるようにした。なお、このような補正の具体的な手順については、後述する。

〈多関節ロボット及びその制御系の構成〉
図１２は、ロボットの位置制御に関する機能ブロック図であり、ロボットメカ１１とロボット制御装置２０の内部構成の概略を示す。

図１２に示すように、ロボットメカ１１は、ロボット１の機械的駆動部であり、サーボモータ１２（以下、単にモータという）と、減速機１３と、エンコーダ１４とを有している。また、図示しないがロボットアーム２を含んでいる。

モータ１２は、減速機１３を介してロボット１の関節部Ｊ１～Ｊ６にそれぞれ連結されており、ロボット制御装置２０のサーボ制御部２３から送られる制御信号により関節部Ｊ１～Ｊ６を駆動し、ロボットアーム２の動作及ぶ姿勢を制御する。ロボット１では、関節部Ｊ１～Ｊ６を動作させて、ロボットアーム２の先端で円や楕円等の所望の軌道を描くことができる。

エンコーダ１４は、モータ１２に接続され、その回転量や回転速度を検出して、検出信号がフィードバック信号としてサーボ制御部２３に送られる。

なお、以降の説明において、関節部Ｊ１に連結されたモータ１２と減速機１３とエンコーダ１４とを、それぞれ「第１のモータ」、「第１の減速機」、「第１のエンコーダ」と呼ぶことがあり、関節部Ｊ２～Ｊ６に連結されたモータ等を「第２～第６のモータ」等と呼ぶことがある。

また、第１のモータに接続されたサーボ制御部２３とバックラッシュ補正ブロック２５とを、それぞれ「第１のサーボ制御部」、「第１のバックラッシュ補正ブロック」と呼ぶことがあり、第２～第６のモータ１２に接続されたサーボ制御部２３と第２～第６のサーボ制御部２３に接続されたバックラッシュ補正ブロック２５とを、それぞれ「第２～第６のサーボ制御部」、「第２～第６のバックラッシュ補正ブロック」と呼ぶことがある。

また、各軸に送られる位置指令や位置指令補正値を、位置指令θ１ｃ～θ６ｃや位置指令補正値θ１ＢＬ～θ６ＢＬと呼ぶことがある。

ロボット制御装置２０は、操作・教示部２１と、メイン制御部２２と、サーボ制御部２３と、バックラッシュ補正ブロック２５とを有している。

操作・教示部２１は、例えば、ティーチング時に取得されたロボットアーム２の軌跡及び、この軌跡を描くための各モータ１２の回転動作等を記憶する。

メイン制御部２２は、操作・教示部２１からの指示を受け、操作・教示部２１に記憶されたロボットメカ１１のロボットアーム２等の移動の軌跡に従い、ロボット１の各軸Ｊ１～Ｊ６の位置指令θ１ｃ～θ６ｃをそれぞれ出力する。

サーボ制御部２３は、メイン制御部２２から送られた位置指令θ１ｃ～θ６ｃに追従するように、ロボットメカ１１内の第１～第６のモータ１２の回転動作をそれぞれ制御する。

バックラッシュ補正ブロック２５は、関節部Ｊ１～Ｊ６に対応して、メイン制御部２２と、各サーボ制御部２３との間にそれぞれ設けられている。バックラッシュ補正ブロック２５は、メイン制御部２２から受け取った位置指令θ１ｃ～θ６ｃに基づいて、位置指令補正値θ１ＢＬ～θ６ＢＬを生成する。生成された位置指令補正値θ１Ｂｌ～θ６ＢＬは、対応する位置指令θ１ｃ～θ６ｃに加算されて第１～第５のサーボ制御部２３にそれぞれ送られる。

なお、ロボット制御装置２０内の各機能ブロックは、それぞれ独立の回路で構成されていてもよいし、一つの集積回路で構成されていてもよい。一部の機能ブロックの組み合わせを一つの集積回路で構成してもよい。

また、メイン制御部２２、サーボ制御部２３、バックラッシュ補正ブロック２５の機能は、ソフトウェアで記述されたプログラムをＣＰＵ等の集積回路上で実行して概ね実現される。

図１３は、ロボットメカ１１から、モータ１２と、減速機１３と、これらに連結されるロボットアーム２の一部を負荷３０として抽出した図である。

負荷３０は、モータ取り付けベースとなる第１アーム３１と、第１アーム３１に連結されたモータ１２と、モータ１２に連結された１次側３２とベアリング３４を有する２次側３３とを含む減速機１３と、減速機１３の２次側３３に回転可能に連結された第２アーム３５とを含んでいる。

減速機１次側３２は、モータ１２の回転軸を介してモータ１２のロータ３６に結合され、サーボ制御部２３から送られたモータ回転位置θＭの分だけ回転する。減速機１３は、モータ回転位置θＭを、減速比Ｒｇでアーム回転位置θＬに変換する。減速機１３は、減速機１次側３２と減速機２次側３３のと間にバネ成分３７が存在する。

なお、位置指令θｃは、各軸の回転角度量を指示する角度指令を示し、実位置θＬは、実際に各軸が回転した回転角度量を示す。

〈多関節ロボットの制御ブロックの詳細構成〉
図１４は、第１のサーボ制御部のブロック線図を示す。図１４に示すように、サーボ制御部２３の位置制御ブロック５０において、位置指令θｃとバックラッシュ補正ブロック２５から出力されるバックラッシュ補正値θＢＬとを加算した値からモータ位置θＭを差分し、位置比例ゲインＫｐｐを乗じて速度指令ωｃｐを生成する。モータ位置θＭは、位置検出器である第１のエンコーダ１４での検出信号より得られる。

なお、以降の説明において、第１のサーボ制御部２３の構成等を例にとって説明するが、第２～第５のサーボ制御部２３についても同様である。

速度制御ブロック５１において、速度指令ωｃｐからモータ位置θＭを微分して得られるモータ速度ωＭを差分した値に速度比例ゲインＫｐｓを乗じた値と、当該差分値を積分して速度積分ゲインＫｉを乗じた値とを加算して、第１のモータ１２に流す電流ＩＭを演算する。電流ＩＭは、負荷３０に入力される。

負荷３０において、ＩＭはモータ１２を駆動するモータ電流指令、Ｋｔはモータ１２のトルク定数、１／Ｒｇは減速比の逆数、Ｋｓは減速機１３のバネ定数、θｓは減速機１次側３２と減速機２次側３３との間に発生するねじれ量、τｄｄｙｎはロボットアーム２に加わる動力学トルクである。

モータ伝達関数４０及び負荷伝達関数４１は、それぞれの物理現象を数式化（モデル化）したものである。

モータ伝達関数４０において、ＪＭはモータ１２のロータ３６と減速機１次側３２とを合わせた回転軸回りの慣性モーメント、ＤＭは粘性摩擦係数である。

負荷伝達関数４１において、ＪＬは第２アーム３５と減速機２次側３３とを合わせた回転軸回りの慣性モーメント、ＤＬは粘性摩擦係数である。

なお、図１４は、負荷及び減速機が連結されたモータの一般的な制御ブロック線図であるため、上記以外の機能の詳細説明は省略する。

図１５は、バックラッシュ補正ブロックの詳細構成を示す。バックラッシュ補正ブロック２５は、動力学演算ブロック６０と、角速度演算ブロック６１と、指令速度成分反転検出ブロック６２と、反転時トルク検出ブロック６３と、バックラッシュ補正量演算ブロック６４とを有している。

動力学演算ブロック６０では、全軸に対してメイン制御部２２から送られた位置指令θ１ｃ～θ６ｃを用い、動力学トルクτｄｙｎを演算する。動力学演算ブロック６０では、全軸の位置指令θ１ｃ～θ６及びその微分値である速度成分、２階微分値である加速度成分から動力学演算を行い、各軸にかかる動力学トルクτｄｙｎを演算している。

補正対象である関節部Ｊ１の位置指令θ１ｃは、角速度演算ブロック６１において、その微分値である角速度成分ｄθｃが演算され、指令速度成分反転検出ブロック６２に入力される。指令速度成分反転検出ブロック６２では、角速度成分ｄθｃに基づいて、反転タイミングｄｉｒが演算される。反転タイミングｄｉｒは、反転時トルク検出ブロック６３と、バックラッシュ補正量演算ブロック６４に入力される。

反転時トルク検出ブロック６３では、動力学トルクτｄｙｎと、反転タイミングｄｉｒとに基づいて、反転時トルクτｄｉｒが演算される。

バックラッシュ補正量演算ブロック６４では、動力学トルクτｄｙｎと、反転時トルクτｄｉｒと、反転タイミングｄｉｒとに基づいて、関節部Ｊ１の補正量θ１ＢＬが演算される。

なお、バックラッシュ補正のみを行う場合には、例えば、図６Ｃに示すバックラッシュ補正量（補正量１（θＢＬ１））を含む波形が補正量θ１ＢＬとして出力される。ここで、バックラッシュ補正量は、実験などによって予め決められた値である。

一方、バックラッシュ補正及びねじれ補正を行う場合には、例えば、後述する図２０Ｃに示す補正量１（θＢＬ１）、ねじれ補正量（補正量２（θＢＬ２））、及び補正時間ｔＢＬ２の波形が補正量θ１ＢＬとして出力される。

そして、メイン制御部２２から第１のサーボ制御部２３に送られる位置指令θ１ｃに、バックラッシュ補正量演算ブロック６４で演算された補正量θ１ＢＬが加味された新たな位置指令が第１のサーボ制御部２３に入力され、第１のサーボ制御部２３によって第１のモータ１２の駆動が制御される。

第１のサーボ制御部２３では、反転時トルクτｄｉｒの大きさに応じて、補正量を調整するようにしている。具体的に、上述した直径１０ｍｍの円を０．２ｍ／分で切断する作業（図２参照）であれば、反転時トルクτｄｉｒが所定のトルク閾値τｔｈよりも小さい（図６Ｂ参照）ので、関節部Ｊ１に生じるねじれの影響が小さく、バックラッシュ補正のみを行うようにすればよい。

一方、上述した直径１０ｍｍの円を２ｍ／分で切断する作業（図７参照）であれば、反転時トルクτｄｉｒが所定のトルク閾値τｔｈよりも大きい（図１１Ｂ参照）ので、関節部Ｊ１に生じるねじれの影響が大きく、バックラッシュ補正に加えて、ねじれ補正も行う必要がある。

図１６に示すように、例えば、直径１０ｍｍの円を２ｍ／分で切断する作業を仮定する。図１６に示す例では、０°の位置から反時計回り方向に円軌道でレーザ切断を行うものとする。

ここで、関節部Ｊ１のモータ１２の回転方向は、ロボットアーム２の先端が、図１６で９０°と２７０°の位置に到達するタイミングでそれぞれ反転する。そのため、９０°と２７０°の位置で、それぞれバックラッシュ誤差が発生する。

具体的に、実軌跡は、上述した図７に示す例と同様に、図１６で反転位置よりも右側が下方向にシフトし、反転位置よりも左側が上方向にシフトした軌跡となっている。

そして、図１６に示す例では、モータ１２の回転速度が速いため、反転時トルクが大きくなって所定のトルク閾値を超えてしまう（図２０Ｂも参照）。そのため、モータ１２の減速機１３にねじれが生じてしまい、このねじれの影響により、反転後の軌跡ずれがさらに大きくなっている。

そこで、図１７に示すように、位置指令に対して、バックラッシュ補正量（補正量１）及びねじれ補正量（以下、「補正量２」と呼ぶ）を加算又は減算することで、実軌跡が目標軌跡に近付くように補正する。

まず、０°から９０°位置にかけて、補正量１（θＢＬ１）を位置指令に加算して、図１７で上方向にシフトさせた位置指令に補正する。

また、９０°から２７０°位置にかけて、補正量１（θＢＬ１）を位置指令から減算して、図１７で下方向にシフトさせた位置指令に補正する。このとき、モータ１２の反転直後である９０°から１８０°の区間において、モータ１２の反転後、所定の補正時間ｔＢＬ２が経過するまでの間は、補正量１（θＢＬ１）よりも大きな補正量２（θＢＬ２）を位置指令から減算して、さらに下方向にシフトさせた位置指令に補正する。

また、２７０°から３６０°（０°）位置にかけて、補正量１（θＢＬ１）を位置指令に加算して、図１７で上方向にシフトさせた位置指令に補正する。このとき、モータ１２の反転直後である２７０°から３６０°（０°）の区間において、モータ１２の反転後、所定の補正時間ｔＢＬ２が経過するまでの間は、補正量１（θＢＬ１）よりも大きな補正量２（θＢＬ２）を位置指令に加算して、さらに上方向にシフトさせた位置指令に補正する。

このように、図１８に示すような補正加算指令を与えることで、ロボットアーム２の実軌跡が円軌道となるように補正することができる。

以下、図１９Ａ及び図１９Ｂに示す測定波形を参照しながら、補正前後で軌跡ずれがどの程度変化したかについて説明する。なお、図１９Ａ及び図１９Ｂに示す測定波形では、誤差形状を明確にするために、誤差を５倍に拡大（デフォルメ）して表示している。図１９Ａは、バックラッシュ補正を行わない場合の、目標軌跡と実軌跡との最大誤差を示している。図１９Ａに示す例では、最大誤差が０．１８３ｍｍとなっている。

一方、図１９Ｂに示すように、バックラッシュ補正を行った場合には、最大誤差が０．０８９ｍｍに低減されていることが分かる。

ここで、直径１０ｍｍの円を２ｍ／分で切断する作業では、指令速度成分、動力学トルク、補正量の時間変化は、図２０Ａ～図２０Ｃに示すグラフ図で表される。

このように、直径１０ｍｍの円を２ｍ／分で切断する条件下では、レーザ切断速度が速いため、図２０Ｂに示すように、反転時トルクτｄｉｒが所定のトルク閾値τｔｈを越えている。

ところで、図２１に示すように、反転時トルクτｄｉｒとねじれ補正量である補正量２（θＢＬ２）とは、反転時トルクτｄｉｒが大きくなるほど、補正量２（θＢＬ２）が大きくなるという関係にある。

また、図２２に示すように、反転時トルクτｄｉｒと補正時間ｔＢＬ２とは、反転時トルクτｄｉｒが大きくなるほど、補正時間ｔＢＬ２が小さくなるという関係にある。

そこで、例えば、図２３Ａ～図２３Ｃに示すように、直径１０ｍｍの円を１ｍ／分で切断する作業について検討すると、この作業は、図２０Ａ～図２０Ｃに示す作業（直径１０ｍｍの円を２ｍ／分で切断）よりもモータ１２の回転速度が遅いため、反転時トルクτｄｉｒが小さくなる。そのため、位置指令に加算又は減算させる補正量２（θＢＬ２）の大きさが小さく、補正時間ｔＢＬ２が長くなるようにすればよい。

以上のように、本実施形態に係るロボット制御装置２０によれば、反転時トルクが所定の閾値よりも大きい場合に、補正対象の関節部Ｊ１のバックラッシュに加えて、ねじれについても補正することで、ロボットアーム２の軌跡誤差を低減することができる。

《実施形態２》
図２４は、本実施形態２に係るロボット制御装置のブロック線図である。以下、前記実施形態１と同じ部分については同じ符号を付し、相違点についてのみ説明する。

図２４に示すように、バックラッシュ補正ブロック７０には、補正対象である関節部Ｊ１（旋回軸）の位置指令θ１ｃのみが入力される。

ここで、関節部Ｊ１には、重力が印加されず、さらに、軌跡精度が必要な動作速度は、１０ｍ／分以下（最大速度の１／２０以下）である。そのため、動力学トルクτｄｙｎの主要成分は、以下の（１）式に示すように、自軸の慣性力となる。

τｄｙｎ≒Ｊ×αＣ ・・・（１）
ここで、Ｊは、旋回軸回りのイナーシャである。イナーシャは、作業時のロボット１の姿勢に依存する。また、αＣは、加速度成分である。加速度成分αＣは、角速度成分ｄθｃの微分値である。

図２５は、バックラッシュ補正ブロックの詳細構成を示す。バックラッシュ補正ブロック７０は、角速度演算ブロック７１と、加速度演算ブロック７２と、指令速度成分反転検出ブロック７３と、反転時加速度検出ブロック７４と、バックラッシュ補正量演算ブロック７５とを有している。

角速度演算ブロック７１では、旋回軸の位置指令θ１ｃの微分値である角速度成分ｄθｃを演算する。角速度演算ブロック７１で演算された角速度成分ｄθｃは、加速度演算ブロック７２と、指令速度成分反転検出ブロック７３に入力される。

加速度演算ブロック７２では、角速度成分ｄθｃの微分値である加速度成分αＣを演算する。加速度演算ブロック７２で演算された加速度成分αＣは、反転時加速度検出ブロック７４と、バックラッシュ補正量演算ブロック７５に入力される。

指令速度成分反転検出ブロック７３では、角速度成分ｄθｃに基づいて、反転タイミングｄｉｒが演算される。反転タイミングｄｉｒは、反転時加速度検出ブロック７４と、バックラッシュ補正量演算ブロック７５に入力される。

反転時加速度検出ブロック７４では、加速度成分αＣと、反転タイミングｄｉｒとに基づいて、反転時加速度αｄｉｒが演算される。

バックラッシュ補正量演算ブロック７５では、加速度成分αＣと、反転時加速度αｄｉｒと、反転タイミングｄｉｒとに基づいて、関節部Ｊ１の補正量θ１ＢＬが演算される。

なお、バックラッシュ補正のみを行う場合には、バックラッシュ補正量（補正量１（θＢＬ１））を含む波形が補正量θ１ＢＬとして出力される。ここで、バックラッシュ補正量は、実験などによって予め決められた値である。

一方、バックラッシュ補正及びねじれ補正を行う場合には、例えば、後述する図２６Ｃに示す補正量１（θＢＬ１）、ねじれ補正量（補正量２（θＢＬ２））、及び補正時間ｔＢＬ２の波形が補正量θ１ＢＬとして出力される。

そして、メイン制御部２２から第１のサーボ制御部２３に送られる位置指令θ１ｃに、バックラッシュ補正量演算ブロック６４で演算された補正量θ１ＢＬが加味された新たな位置指令が第１のサーボ制御部２３に入力され、第１のサーボ制御部２３によって第１のモータ１２の駆動が制御される。

第１のサーボ制御部２３では、反転時加速度αｄｉｒの大きさに応じて、補正量を調整するようにしている。具体的に、上述した直径１０ｍｍの円を０．２ｍ／分で切断する作業（図２参照）であれば、反転時加速度αｄｉｒが所定の加速度閾値αｔｈよりも小さいので、関節部Ｊ１に生じるねじれの影響が小さく、バックラッシュ補正のみを行うようにすればよい。

一方、上述した直径１０ｍｍの円を２ｍ／分で切断する作業（図７参照）であれば、指令速度成分、動力学トルク、補正量の時間変化は、図２６Ａ～図２６Ｃに示すグラフ図で表される。

ここで、直径１０ｍｍの円を２ｍ／分で切断する条件下では、レーザ切断速度が速いため、図２６Ｂに示すように、反転時加速度αｄｉｒが所定の加速度閾値αｔｈを越えている。

そのため、関節部Ｊ１に生じるねじれの影響が大きく、図２６Ｃに示すように、バックラッシュ補正に加えて、ねじれ補正も行うようにすればよい。

ところで、図２７に示すように、反転時加速度αｄｉｒとねじれ補正量である補正量２（θＢＬ２）とは、反転時加速度αｄｉｒが大きくなるほど、補正量２（θＢＬ２）が大きくなるという関係にある。

また、図２８に示すように、反転時加速度αｄｉｒと補正時間ｔＢＬ２とは、反転時加速度αｄｉｒが大きくなるほど、補正時間ｔＢＬ２が小さくなるという関係にある。

そこで、例えば、直径１０ｍｍの円を１ｍ／分で切断する作業について検討すると、この作業は、図２６Ａ～図２６Ｃに示す作業（直径１０ｍｍの円を２ｍ／分で切断）よりもモータ１２の回転速度が遅いため、反転時加速度αｄｉｒが小さくなる。そのため、位置指令に加算又は減算させる補正量２（θＢＬ２）の大きさが小さく、補正時間ｔＢＬ２が長くなるようにすればよい。

なお、ロボット１の姿勢による旋回軸回りのイナーシャの変化に対しては、図２７及び図２８に示す補正量２（θＢＬ２）及び補正時間ｔＢＬ２のテーブルを複数組用意することで対応すればよい。

以上のように、本実施形態に係るロボット制御装置２０によれば、反転時加速度が所定の閾値よりも大きい場合に、補正対象の関節部Ｊ１のバックラッシュに加えて、ねじれについても補正することで、ロボットアーム２の軌跡誤差を低減することができる。

《その他の実施形態》
前記実施形態については、以下のような構成としてもよい。

本実施形態では、垂直多関節６軸のロボット１を例にとって説明したが、特にこれに限定されない。ロボットの関節部の数は６つに限られず、ロボットの仕様等によって適宜変更されうる。

以上説明したように、本発明は、ロボットアームの軌跡誤差を低減することができるという実用性の高い効果が得られることから、きわめて有用で産業上の利用可能性は高い。

１ ロボット
２ ロボットアーム
２０ ロボット制御装置
６１ 角速度演算ブロック（角速度演算部）
６０ 動力学演算ブロック（動力学演算部）
６２ 指令速度成分反転検出ブロック（反転タイミング演算部）
６３ 反転時トルク検出ブロック（反転時トルク演算部）
６４ バックラッシュ補正量演算ブロック（補正量演算部）
７１ 角速度演算ブロック（角速度演算部）
７２ 加速度演算ブロック（加速度演算部）
７３ 指令速度成分反転検出ブロック（反転タイミング演算部）
７４ 反転時加速度検出ブロック（反転時加速度演算部）
７５ バックラッシュ補正量演算ブロック（補正量演算部）
Ｊ１ 関節部（旋回軸）
Ｊ２～Ｊ６ 関節部

Claims (8)

1. 複数の関節部を有するロボットアームの動作を制御するためのロボット制御方法であって、
   前記複数の関節部のうち、補正対象の該関節部の角速度を演算するステップと、
   前記複数の関節部の位置指令に基づいて、前記補正対象の関節部にかかるトルクを演算するステップと、
   前記補正対象の関節部の角速度に基づいて、該関節部の回転方向が反転する反転タイミングを演算するステップと、
   前記トルクと前記反転タイミングとに基づいて、前記補正対象の関節部にかかる反転時トルクを演算するステップと、
   前記反転時トルクと前記反転タイミングとに基づいて、該反転時トルクによって該補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を演算するステップとを備えたことを特徴とするロボット制御方法。
2. 請求項１において、
   前記反転時トルクが所定の閾値よりも小さい場合には、前記補正対象の関節部で生じるバックラッシュを補正するためのバックラッシュ補正量に基づいて該関節部の位置指令を補正する一方、該反転時トルクが所定の閾値よりも大きい場合には、該バックラッシュ補正量と前記ねじれ補正量及び補正時間に基づいて該関節部の位置指令を補正するステップを備えたことを特徴とするロボット制御方法。
3. 請求項１又は２において、
   前記ねじれ補正量及び補正時間を演算するステップでは、前記反転時トルクが大きいほど、該ねじれ補正量が大きく且つ該補正時間が短くなるように、該ねじれ補正量及び該補正時間が演算されることを特徴とするロボット制御方法。
4. 複数の関節部を有するロボットアームの動作を制御するためのロボット制御方法であって、
   前記複数の関節部のうち補正対象の該関節部は、前記ロボットアームを旋回させる旋回軸であり、
   前記旋回軸の位置指令に基づいて、該旋回軸の角速度を演算するステップと、
   前記旋回軸の角速度に基づいて、該旋回軸の加速度を演算するステップと、
   前記旋回軸の角速度に基づいて、該旋回軸の回転方向が反転する反転タイミングを演算するステップと、
   前記旋回軸の加速度と前記反転タイミングとに基づいて、該旋回軸の反転時加速度を演算するステップと、
   前記反転時加速度と前記反転タイミングとに基づいて、前記旋回軸の反転時に該旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を演算するステップとを備えたことを特徴とするロボット制御方法。
5. 請求項４において、
   前記反転時加速度が所定の閾値よりも小さい場合には、該旋回軸で生じるバックラッシュを補正するためのバックラッシュ補正量に基づいて該旋回軸の位置指令を補正する一方、該反転時加速度が所定の閾値よりも大きい場合には、該バックラッシュ補正量と前記ねじれ補正量及び補正時間に基づいて該旋回軸の位置指令を補正するステップを備えたことを特徴とするロボット制御方法。
6. 請求項４又は５において、
   前記ねじれ補正量及び補正時間を演算するステップでは、前記反転時加速度が大きいほど、該ねじれ補正量が大きく且つ該補正時間が短くなるように、該ねじれ補正量及び該補正時間が演算されることを特徴とするロボット制御方法。
7. 複数の関節部を有するロボットアームの動作を制御するためのロボット制御装置であって、
   前記複数の関節部のうち、補正対象の該関節部の角速度を演算する角速度演算部と、
   前記複数の関節部の位置指令に基づいて、前記補正対象の関節部にかかるトルクを演算する動力学演算部と、
   前記補正対象の関節部の角速度に基づいて、該関節部の回転方向が反転する反転タイミングを演算する反転タイミング演算部と、
   前記トルクと前記反転タイミングとに基づいて、前記補正対象の関節部にかかる反転時トルクを演算する反転時トルク演算部と、
   前記反転時トルクと前記反転タイミングとに基づいて、該反転時トルクによって該補正対象の関節部で生じるねじれを補正するためのねじれ補正量及び補正時間を演算する補正量演算部とを備えたことを特徴とするロボット制御装置。
8. 複数の関節部を有するロボットアームの動作を制御するためのロボット制御装置であって、
   前記複数の関節部のうち補正対象の該関節部は、前記ロボットアームを旋回させる旋回軸であり、
   前記旋回軸の位置指令に基づいて、該旋回軸の角速度を演算する角速度演算部と、
   前記旋回軸の角速度に基づいて、該旋回軸の加速度を演算する加速度演算部と、
   前記旋回軸の角速度に基づいて、該旋回軸の回転方向が反転する反転タイミングを演算する反転タイミング演算部と、
   前記旋回軸の加速度と前記反転タイミングとに基づいて、該旋回軸の反転時加速度を演算する反転時加速度演算部と、
   前記反転時加速度と前記反転タイミングとに基づいて、前記旋回軸の反転時に該旋回軸で生じるねじれを補正するためのねじれ補正量及び補正時間を演算する補正量演算部とを備えたことを特徴とするロボット制御装置。

Images