(57)【特許請求の範囲】
【請求項１】
与えられたサーボ指令値に追従するようにロボットの各駆動部の位置を制御する駆動部位置サーボ手段を備え、該駆動部位置サーボ手段は駆動部位置フィードバックループと駆動部速度フィードバックループとを備えて構成されるロボット制御装置において、上記ロボットの各駆動部の位置指令値と該各駆動部の現在位置との差を計算する位置誤差計算手段と、上記位置誤差計算手段の出力と任意の値に設定された位置ゲイン補正係数との積を計算する補正量計算手段と、上記補正量計算手段の出力に上記各駆動部の現在位置を加算する加算手段と、上記ロボットの各駆動部の速度に任意に設定された速度ゲイン補正係数を乗ずる速度ゲイン補正手段と、上記速度ゲイン補正手段の出力と上記加算手段の出力とを加算して補正指令値を計算する補正指令値計算手段とを備え、上記補正指令値計算手段の出力を各関節のサーボ指令値として上記駆動部位置サーボ手段に入力することを特徴とするロボット制御装置。
【請求項２】
各駆動部の位置指令値の時間微分に速度フィードフォワード係数を乗ずる速度フィードフォワード手段を付加し、上記補正指令値計算手段において、上記加算手段の出力と上記速度ゲイン補正手段の出力と上記速度フィードフォワード手段の出力とを加算して補正指令値を計算することを特徴とするロボット制御手段を備えたことを特徴とする請求項１記載のロボット制御装置。
【請求項３】
与えられたサーボ指令値に追従するようにロボットの各駆動部の位置を制御する駆動部位置サーボ手段を備え、該駆動部位置サーボ手段は駆動部位置フィードバック手段と駆動部速度フィードバック手段とを備えて構成されるロボット制御装置において、任意に設定された直交座標系におけるロボットの現在位置を計算する座標変換手段と、上記直交座標系におけるロボットの移動速度を計算する速度計算手段と、上記直交座標系を基準として与えられた直交位置指令値と上記ロボットの現在位置との差を計算する位置誤差計算手段と、上記位置誤差計算手段の出力と任意に設定された位置ゲイン補正係数との積を計算する補正量計算手段と、上記補正量計算手段の出力に上記ロボットの現在位置を加算する加算手段と、上記ロボットの移動速度に速度ゲイン補正係数を乗ずる速度ゲイン補正手段
と、上記速度ゲイン補正手段の出力と上記加算手段の出力とを加算して補正指令値を計算する直交補正指令値計算手段と、上記直交補正指令値計算手段の出力を駆動部位置指令値に変換する逆変換手段とを備え、上記逆変換手段の出力を各駆動部のサーボ指令値として上記駆動部位置サーボ手段に入力することを特徴とするロボット制御装置。
【請求項４】
直交位置指令値の時間微分に速度フィードフォワード係数を乗ずる速度フィードフォワード手段と、上記加算手段の出力と上記速度ゲイン補正手段の出力と上記速度フィードフォワード手段の出力とを加算して上記直交補正指令値を計算する直交補正指令値計算手段を備えたことを特徴とする請求項３記載のロボット制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は産業用等に広く使用されているロボットの制御装置に関するものである。
【０００２】
【従来の技術】
図６は一般的なロボット制御装置を示すブロック図である。図において、１は各関節の位置指令値を生成する位置指令値生成手段、１０１はロボットの第１関節の位置を制御する第１関節位置サーボ手段、１０２はロボットの第２関節の位置を制御する第２関節位置サーボ手段、１０３はロボットの第３関節の位置を制御する第３関節位置サーボ手段である。また、５はロボットの関節を駆動するモータ、６はモータの位置を検出する位置検出器、７は位置検出器６で検出された位置を微分して関節速度を出力する速度検出器である。さらに、２は関節位置サーボの位置フィードバックループにおける位置ゲインＫｐ、３、４はそれぞれ、関節速度フィードバックループにおける速度ゲインＫｖ、および積分器で、Ｋｉは積分ゲインである。また、８は異常検知手段である。
【０００３】
図６では第１関節サーボ手段１０１の構成のみを詳しく描いているが、第２関節サーボ手段１０２、第３関節サーボ手段１０３も同様に構成されている。また、図６では、３個の関節から構成されるロボットの例を示しているが、関節の個数が３個以外ロボットの場合は関節位置サーボ手段の個数が関節の数に対応して変化するだけで、同様に構成される。図６以外の図においても同様である。
【０００４】
次に、図６に示した制御装置の作用を第１関節を例に説明する。指令値生成手段１で生成された第１関節位置指令値は第１関節位置サーボ手段１０１に入力される。この位置指令値と位置検出器６で検出された第１関節の現在位置との差が位置誤差となり、これに位置ゲイン２を乗ずることにより速度指令値を求める。この速度指令値と速度検出器７で検出された関節速度との差が速度偏差となる。この速度偏差に速度ゲインＫｖを乗じた値と、これを積分器４で積分し、積分ゲインＫｉを乗じた値とを加算することによりトルク指令を求め、該トルク指令によりモータ５を駆動する。
【０００５】
また、通常の関節位置サーボ手段では、安全性を高めるため、いくつかの異常検知手段が設けられている。８の異常検知手段はその一つを示したものであり、指令値と実際のモータ位置との位置誤差が設定値よりも大きくなった場合には関節位置サーボ手段１０１に何らかの異常が発生したものと判断して、ロボットを緊急停止させるなどの処置を取るようになっている。
【０００６】
従来のロボット制御装置は上記のように構成され、指令値生成手段１で生成された位置指令値に精度よく追従するように、ロボットの各関節位置を制御している。一般に、位置制御ゲインＫｐ、および、速度制御ゲインＫｖを大きく設定する程、高速・高精度の制御が実現できることから、ロボット制御装置においても、Ｋｐ、Ｋｖを出来るだけ大きく設定することにより、指令値に高速・高精度に追従する制御を実現している。
【０００７】
しかし、このような制御ではロボットが何らかの原因で外部の物体に接触した場合などには作業を続行することが不可能となる場合がある。これは、接触している物体が障害物となり、ロボットが指令された位置に到達できないにもかかわらず、制御装置は、この物体に抗して、ロボットを指令された位置に移動させようとするためである。そのため、過大なモータトルクが発生し、ロボットやワークの破損、または、ロボットの異常停止を招くことになる。特に、嵌め合い作業などでは、ロボットが把持している部品と外部に固定されたワークとが接触することが前提となっているため、ワークの位置が少しでもずれていると作業が出来なくなる。
【０００８】
このような不都合を回避するため、たとえば、特開昭６３−１３９６７８に開示されているようなコンプライアンス制御が用いられる。この先行技術に示されたコンプライアンス制御は、力センサを用いてロボットの先端に作用する外力を検出し、検出した外力に基づいてロボットの動作を制御するものである。これにより、ロボットが外部の物体に接触した場合でも、接触力を検出しながらロボットを制御するので、過大なモータトルクを発生することなく、柔軟な制御を達成することができる。しかし、この先行技術では構造が複雑で高価な力センサを必要とすると言う問題点があった。
【０００９】
他の先行技術は、たとえば、特開平６−３３２５３８号公報に開示されている。この先行技術では、力センサを用いずにコンプライアンス制御と同等の機能を実現している。図７は特開平６−３３２５３８号公報に開示された従来技術によるコンプライアンス制御方法を示すブロック図である。図７は第１関節の関節位置サーボ手段のみを示したものであり、図６と同一、または相当部分には同一の番号を付して示している。
【００１０】
この制御方法では、ロボットの各関節を駆動する関節位置サーボ手段のゲインを変更可能とし、必要に応じて、位置ゲインＫｐ、および、速度ゲインＫｖを小さく設定することにより、過大なモータトルクの発生を防止するものである。位置ゲインＫｐを小さくすると、位置偏差に該ゲインＫｐが乗じられて得られる速度指令値も通常の制御時よりも小さな値となる。これにともない、該速度指令とモータ速度との差である速度偏差も通常時よりも小さくなり、さらに速度ゲインＫｖも小さな値に設定されているので、速度偏差に速度ゲインＫｖを乗じた値も小さくなる。また、速度制御ループの積分器の出力は小さな値にクランプされて出力されるようになっており、このため、トルク指令は大きな値となることはなく、過大なモータトルクが発生することがなくなる。
【００１１】
これにより、ロボットが外部の障害物に接触して位置誤差が大きくなった場合でも、過大な力が作用することがなくなり、ロボットやワークを破損することなく作業を続行することが出来るようになる。また、嵌め合い作業で、ワークの位置がずれているような場合でも、ワークとロボットとの接触によって生じる外力によりロボットの各関節が受動的に変位させられ、ワーク位置のずれに合わせてロボットの位置が補正されることになる。これにより、嵌め合い作業を円滑に行うことができるようになる。
【００１２】
【発明が解決しようとする課題】
上記のように、従来技術では、必要に応じて各関節の位置サーボ手段の位置ゲインＫｐと速度ゲインＫｖを小さな値に変更するように構成されており、これにより、過大なモータトルクが発生するのを防止している。また、ロボットに外部から力が作用した場合には、この外力により各関節が受動的に変位することにより、ワークの位置ずれを吸収するように構成されていた。したがって、ロボットに大きな外力が作用する場合には、各関節のサーボ系に与えられる指令値と実際のモータ位置との偏差が大きくなるが、これは、該制御方法が正常に作用しているためである。図６に示した通常のロボット制御装置では、安全性を高めるため、上記のように指令値と実際のモータ位置との偏差が大きくなった場合にはサーボ制御装置に何らかの異常があるものと判断している。一方、図７に示した制御方法では、サーボ制御手段が正常に作用している場合にも上記のように大きな偏差が出ることがあるため、この異常検知が出来なくなり、安全性が低下すると言う問題があった。
【００１３】
また、図６に示した通常のロボット制御装置では、与えられた位置指令値に高速・高精度に追従する制御を実現するため、位置ゲインＫｐ、および、速度ゲインＫｖを出来るだけ大きな値に設定していた。ところが、図７に示す制御方法では、位置ゲインＫｐと速度ゲインＫｖを小さな値に変更するため、指令値への追従が高精度には出来なくなると言う問題点があった。
【００１４】
本発明は、図７に示した先行技術の問題点を解決するためになされたものであり、図７で実現されていたコンプライアンス制御機能を、安全性を犠牲にすることなく実現し、同時に指令値への高精度な追従を可能とするロボット制御装置を提供するものである。
【００１５】
【課題を解決するための手段】
本発明の第１および第３の構成によるロボット制御装置は、ロボットに外力が作用したとき、外力によるロボットの変位を検知し、該検知された変位に基づいてロボットの位置指令値を補正し、該補正された位置指令値を関節位置サーボ手段にサーボ指令値として与えるものである。
【００１６】
また、発明の第２および第４の構成によるロボット制御装置は、第１および第３の構成によるロボット制御装置に加えて、指令値生成手段で生成された位置指令値の時間微分をフィードフォワード項として、サーボ指令値に加算するように構成したものである。
【００１７】
【発明の実施の形態】
実施の形態１．
図１は本発明の実施の形態１であるロボット制御装置を示すブロック図であり、図６と同一または相当部分には同じ番号を付して示している。図において、２０１は第１関節の指令値補正手段であり、以下のように構成される。すなわち、１１は位置指令値生成手段１で生成された位置指令値と位置検出器６で検出されたモータ位置との差を計算する位置誤差計算手段、１２は上記位置誤差計算手段１１の出力と任意の値に設定された位置ゲイン補正係数Ｋｃｐとの積を計算する補正量計算手段、１３は上記補正量計算手段１２の出力に上記各関節の現在位置を加算する加算手段、１４は位置検出器６で検出されたモータ位置を微分することにより関節速度を計算する速度検出手段、１５は上記関節速度に任意に設定された速度ゲイン補正係数Ｋｃｖを乗ずる速度ゲイン補正手段、１６は上記速度ゲイン補正手段１５の出力と上記加算手段１３の出力とを加算して補正指令値を計算する補正指令値計算手段である。上記補正指令値計算手段１６の出力は各関節のサーボ指令値として第１関節位置サーボ手段１０１に入力される。また、２０２は第２関節の指令値補正手段、２０３は第３関節の指令値補正手段であり、上記第１関節の指令値補正手段２０１と同様に構成されている。
【００１８】
図１に示した制御装置の作用を説明するために、該ブロック図を等価変換する。
Ｋｃｐ＝Ｋ１＊Ｋ２
Ｋｃｖ＝（１−Ｋ２）／Ｋｐ
とおいて、第１関節の指令値補正手段２０１と関節位置サーボ手段１０１の部分を等価変換すると図２に示すブロック図となる。図２において、Ｋ１＊Ｋｐを新たに位置ゲインＫｐと見なし、Ｋ２＊Ｋｖを新たに速度ゲインＫｖと見なすと、図２のブロック図と図７に示した従来技術によるブロック図とは同じものとなる。しかも、図２に示す実施の形態では上式のＫ１、Ｋ２を適当に設定することにより、図７の従来技術と同様に位置ゲインと速度ゲインを自由に変更できるようになっている。つまり、図１に示した制御方式では、位置ゲイン補正係数Ｋｃｐと速度ゲイン補正係数Ｋｃｖを変更することにより、関節サーボ手段に含まれる位置ゲインＫｐと速度ゲインＫｖを変更することなく、図７に示した制御方式と全く同じコンプライアンス制御を果たすことができる。
【００１９】
図１に示す制御方式では、関節位置サーボ手段に与えるサーボ指令値を補正することにより、コンプライアンス制御を実現しており、関節位置サーボ手段に含まれる位置ゲインＫｐと速度ゲインＫｖは変更していない。したがって、関節位置サーボ手段は通常の制御手段と同様に、与えられたサーボ指令に対して高速・高精度にモータを追従させるように制御しており、サーボ指令と実際のモータ位置との差である位置誤差が、従来技術によるコンプライアンス制御方法のように大きくなることはない。これにより、位置誤差が大きくなった場合は、関節位置サーボ手段に何らかの異常があるものと判断することができ、図６に示した通常のサーボ制御手段と同様の異常検知を行うことができるようになる。
【００２０】
実施の形態２．
図３は本発明の実施の形態２であるロボット制御装置を示すブロック図であり、図１と同一または相当部分には同じ番号を付して示している。図において、１７は位置指令値の時間微分を求める微分手段であり、１８は上記位置指令値の時間微分に速度フィードフォワード係数Ｋｆを乗ずる速度フィードフォワード手段である。
【００２１】
図３において、
Ｋｃｐ＝Ｋ１＊Ｋ２
Ｋｃｖ＝（１−Ｋ２）／Ｋｐ
Ｋｆ＝Ｋ２／Ｋｐ
とおいて、図１と同様に等価変換すると図４に示すブロック図が得られる。これは、図７に示す従来技術に指令速度のフィードフォワードを付加したものと等価となる。コンプライアンス制御を実現するために位置ゲインＫｐ、速度ゲインＫｖを小さく設定すると関節位置指令値に高精度に追従できなくなるが、本発明では、指令値速度のフィードフォワードを付加することにより、指令値への追従特性を改善することができ、モータ位置を指令値へ高精度に追従させることができるようになる。
【００２２】
なお、この実施の形態では、フィードフォワード係数Ｋｆを、Ｋｆ＝Ｋ２／Ｋｐとしているが、この値に限るものではなく、ロボットの動きを見ながら調整することで、より良い性能を得ることができるようになる。
【００２３】
実施の形態３．
図５は本発明の実施の形態３であるロボット制御装置を示すブロック図であり、図３と同一または相当部分には同じ番号を付して示している。図において、２２は各関節の位置検出器６で検出されたモータ位置を直交座標系におけるロボットの位置に変換する座標変換手段である。
【００２４】
３０１は直交座標における指令値補正手段であり、以下のように構成される。すなわち、１１は位置指令値生成手段１で生成された直交系位置指令値と座標変換手段２２で求めたロボット位置との差を計算する位置誤差計算手段、１２は上記位置誤差計算手段１１の出力と任意の値に設定された位置ゲイン補正行列Ｋｃｐとの積を計算する補正量計算手段、１３は上記補正量計算手段１２の出力に上記直交座標系におけるロボットの位置を加算する加算手段、１４は上記直交座標系におけるロボットの位置を微分することにより直交座標系におけるロボットの速度を計算する速度検出手段、１５は上記直交座標系におけるロボットの速度に任意に設定された速度ゲイン補正行列Ｋｃｖを乗ずる速度ゲイン補正手段、１６は上記速度ゲイン補正手段１５の出力と上記加算手段１３の出力とを加算して直交座標系における補正指令値を計算する補正指令値計算手段である。また、２１は直交座標系における補正指令値を関節座標系に変換する逆変換手段であり、上記補正指令値計算手段１６の出力は逆変換手段により、各関節のサーボ指令値に変換され、第１関節位置サーボ手段１０１〜１０３に入力される。
【００２５】
１０１、１０２、１０３はそれぞれ、第１関節、第２関節、第３関節の関節位置サーボ手段であり、図１における１０１、１０２、１０３と同じものである。また、上記の位置ゲイン補正行列Ｋｃｐ、および、速度ゲイン補正行列Ｋｃｖは直交座標系の各軸方向の柔らかさを指定するパラメータを要素とする対角行列である。
【００２６】
図５に示す実施の形態３は、図３に示す実施の形態２を直交座標系における表現に書き直したものであり、図３とほぼ同様に作用し、コンプライアンス制御を実現すると同時に、指令値へも高精度に追従する制御を実現することができる。
【００２７】
本発明の第１および第２の構成によるロボット制御装置は、駆動部の座標系例えば関節座標系においてコンプライアンス制御を実現するものであり、各関節のサーボ制御の柔らかさを調整するものである。これに対して、実施の形態３に示す本発明の第３および第４によるロボット制御装置は直交座標系でコンプライアンス制御を実現するものであり、直交座標系の各軸方向の柔らかさを独立に調整することができる。たとえば、嵌め合い作業では、挿入方向には剛性を高く設定し、挿入方向と直交する方向には剛性を低く設定して柔らかい制御をするのが良いと言われている。第３および第４の構成によるロボッット制御装置はこのような機能を実現するものであり、位置ゲイン補正行列Ｋｃｐ、および、速度ゲイン補正行列Ｋｃｖの値を変更することにより、直交座標系の各軸方向の柔らかさを任意に設定することができる。
【００２８】
以上の説明は駆動部がロボットの関節である場合について説明したが、本発明はこれに限らずロボットの手、手首など他の駆動部も対象となり得る。
【００２９】
【発明の効果】
本発明の第１および第３の構成によるロボット制御装置によれば、駆動部位置サーボ手段の位置ゲインＫｐと速度ゲインＫｖを変更することなく、該駆動部位置サーボ手段に与えるサーボ指令値を補正することによりコンプライアンス制御を実現しているので、通常のサーボ手段と同様の異常検知を行うことができ、安全性の高い制御装置を得ることができる。
【００３０】
また、本発明の第２および第４の構成によるロボット制御装置によれば、指令速度のフィードフォワードを付加することにより、コンプライアンス制御と指令値への高精度な追従とを同時に実現することができる。
【図面の簡単な説明】
【図１】  本発明の実施の形態１であるロボット制御装置を示すブロック図である。
【図２】  本発明の実施の形態１であるロボット制御装置の作用を説明するためのブロック図である。
【図３】  本発明の実施の形態２であるロボット制御装置を示すブロック図である。
【図４】  本発明の実施の形態２であるロボット制御装置の作用を説明するためのブロック図である。
【図５】  本発明の実施の形態３であるロボット制御装置を示すブロック図である。
【図６】  一般的なロボット制御装置を示すブロック図である。
【図７】  従来技術によるコンプライアンス制御方法を示すブロック図である。
【符号の説明】
１  位置指令値生成手段、２  位置ゲイン、３  速度ゲイン、４  積分器、５モータ、６  位置検出器、７  速度検出器、８  異常検知手段、１１  位置誤差計算手段、１２  補正量計算手段、１３  加算手段、１４  速度検出手段、１５  速度ゲイン補正手段、１６  補正指令値計算手段、１７  微分手段、１８  速度フィードフォワード手段、２１  逆変換装置、２２  座標変換装置、１０１第１関節位置サーボ手段、１０２  第２関節位置サーボ手段、１０３  第３関節位置サーボ手段、２０１  第１関節の指令値補正手段、２０２  第２関節の指令値補正手段、２０３  第３関節の指令値補正手段、３０１  指令値補正手段。
【図１】
0003659045.tif 000002
【図２】
0003659045.tif 000003
【図３】
0003659045.tif 000004
【図４】
0003659045.tif 000005
【図５】
0003659045.tif 000006
【図６】
0003659045.tif 000007
【図７】
0003659045.tif 000008