JP3955217B2

JP3955217B2 - 産業用ロボットの制御方法及び制御装置

Info

Publication number: JP3955217B2
Application number: JP2002038022A
Authority: JP
Inventors: 章丸山
Original assignee: Nachi Fujikoshi Corp; Daihen Corp
Current assignee: Nachi Fujikoshi Corp; Daihen Corp
Priority date: 2002-02-15
Filing date: 2002-02-15
Publication date: 2007-08-08
Anticipated expiration: 2022-02-15
Also published as: JP2003241811A

Description

【０００１】
【発明の属する技術分野】
本発明は、産業用ロボットの制御に関し、特に、ロボットの動作経路を生成するための制御方法及び制御装置に関する。
【０００２】
【従来の技術】
産業用ロボットの動作を制御する場合には、動作経路が高精度に維持され、直線や曲線が混在する動作経路上を円滑に動作させることが重要である。このため、最近では、平滑な加速及び減速を通じて、ロボットの動作経路を計画することが可能な制御装置が採用されている。
【０００３】
平滑な加速及び減速を実現する方法として一般的によく用いられている方法は、加加速度を指定して線速度プロファイルを計算するＳ字カーブ指定法である。図１は、このＳ字カーブ指定法により動作経路を計画することが可能な制御装置の一例を示すブロック図である。また、図２は、図１の装置における、線速度プロファイル１２、線加速度プロファイル１３、線加加速度プロファイル１４、を示すグラフである。図１において、１は、図２に示した線速度プロファイル１２を加加速区間５、定加速区間６、加減速区間７、定速度区間８、減加速区間９、定減速区間１０、減減速区間１１に分け、それら各区間の時間などの諸元を計算するプロファイル計算部である。２は、プロファイル計算部１の線速度プロファイル１２、線加速度プロファイル１３、及び線加加速度プロファイル１４の諸元に基づいて補間点の３次元位置を計算する補間処理部である。３は、補間処理部２の出力である補間点に対して逆キネマティクス演算を施すことによりロボット各関節軸の指令位置に変換する逆キネマティクス演算部である。４は、逆キネマティクス演算部３の出力であるロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるサーボアンプである。
【０００４】
しかし、前述したＳ字カーブ指定法では、図２に示したように、線速度プロファイル１２を加加速区間５、定加速区間６、加減速区間７、定速度区間８、減加速区間９、定減速区間１０、減減速区間１１の７つの区間に分け、これに区間の切り換わり目の境界条件を加えた８つの連立方程式を解くことにより、それぞれの区間の時間を計算する必要があるが、係る計算は複雑なので、この方法を実際に適用することは困難である。
【０００５】
そこで、特開平１１−２４９７２４号公報に開示されているものにおいては、図３に示すように、逆キネマティクス演算部３の出力であるロボット各関節軸の指令位置に対してローパスフィルタ処理を施すローパスフィルタ１８を設けるようにしている。これによれば、図４に示すように、線速度プロファイル２３は加速部２０、定速部２１、及び減速部２２からなるものとして、それぞれの区間の時間を計算すればよいので、前述したＳ字カーブ指定法を利用したものに比して計算も容易になるとされ、さらに、滑らかな起動・停止を行わせ、ロボットに振動が生じないようにすることが可能であるとされている。
【０００６】
【発明が解決しようとする課題】
しかし、この特開平１１−２４９７２４号公報に開示されているものでは、逆キネマティクス演算部３においてロボット各関節軸のそれぞれの指令位置を生成し、これら各関節軸の指令位置のそれぞれに対して個々にローパスフィルタ処理を行っているので、この場合、ロボット各関節軸が個々に平滑化されることになり、その結果、ロボット各関節軸の動きにアンバランスが生じ、ロボットアーム先端の直線軌跡にずれが生じるという問題がある。
【０００７】
本発明は、係る従来技術の問題点を解決するためのものであり、滑らかな起動・停止を行わせることによりロボットの動きを円滑化するとともに、サーボアンプに入力するロボット各関節軸の指令位置に直線軌跡ずれが生じないような、産業用ロボットの制御方法及び制御装置を提供することを目的とする。
【０００８】
【課題を解決するための手段】
係る目的を達成するために、本発明では、予め指定された直線軌跡開始点と終了点とから加速度一定としたときの３次元空間中の線速度プロファイル、線速度プロファイルの１階微分値としての線加速度プロファイル、及び線速度プロファイルの２階微分値としての線加加速度プロファイルを生成し、これら線速度プロファイル、線加速度プロファイル、及び線加加速度プロファイルに対してフィルタ処理を施すことにより、平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成し、これら平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルに基づいて補間周期毎の補間点の３次元位置を生成し、この補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成し、このロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるようにしたことを特徴とする産業用ロボットの制御方法を提供した。
【０００９】
また、係る制御方法を実現する制御装置として、本発明では、予め指定された直線軌跡開始点と終了点とから加速度一定としたときの３次元空間中の線速度プロファイル、線速度プロファイルの１階微分値としての線加速度プロファイル、及び線速度プロファイルの２階微分値としての線加加速度プロファイルを生成するプロファイル計算部と、これら線速度プロファイル、線加速度プロファイル、及び線加加速度プロファイルに対してフィルタ処理を施すことにより、平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成するローパスフィルタと、これら平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルに基づいて補間周期毎の補間点の３次元位置を生成する補間処理部と、この補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成する逆キネマティクス演算部と、このロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるサーボアンプと、を有することを特徴とする産業用ロボットの制御装置を提供した。
【００１０】
係る構成としたことにより、本発明では、教示等により予め指定された直線軌跡開始点と直線軌跡終了点とから、その２点間の３次元ベクトルと加速度一定としたときの線速度プロファイル、線加速度プロファイル、及び線加加速度プロファイル（以下「線速度プロファイル等」と記す）を計算し、低域通過の特性をもつローパスフィルタを利用して線速度プロファイル等を平滑化し、直線軌跡の補間点を平滑化された線速度プロファイル等から計算し、これを逆キネマティクス演算によりロボット各関節軸の指令位置に変換するようにしている。すなわち、本発明では、逆キネマティクス演算においてロボット各関節軸の指令位置を生成する前の段階、具体的には２点間の直線軌跡上の線速度に対してローパスフィルタ処理を行っている。そのため、逆キネマティクス演算部で生成されたロボット各関節軸の指令位置のそれぞれに対して個々にローパスフィルタ処理を行うようにしている従来技術のように、各関節軸の動きにアンバランスが生じることはなく、その結果、ロボットアーム先端の直線軌跡にずれが生じるという問題はなくなる。
【００１１】
【発明の実施の形態】
以下、本発明の一実施形態について、図面を参照しながら説明する。図５は、本発明の一実施形態における、制御装置を示すブロック図である。図５において、２４は、予め教示などにより指定された直線軌跡開始点と終了点から、加速度一定としたときの３次元空間中の線速度プロファイル、線速度プロファイルの１階微分値としての線加速度プロファイル、及び線速度プロファイルの２階微分値としての線加加速度プロファイルを生成し、これらを出力するプロファイル計算部である。２５は、プロファイル計算部２４から線速度プロファイル、線加速度プロファイル、及び線加加速度プロファイルを入力し、これら各プロファイルに対してローパスフィルタ処理を施すことにより、平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルを生成し、これらを出力するローパスフィルタである。
【００１２】
２６は、ローパスフィルタ２５から平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルを入力し、これら平滑化された各プロファイルに基づいて補間周期毎の補間点の３次元位置を生成し、これらを出力する補間処理部である。２７は、補間処理部２６から補間周期毎の補間点の３次元位置を入力し、この補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することにより、ロボット各関節軸の指令位置を生成し、これを出力する逆キネマティクス演算部である。なお、逆キネマティクス演算とは、指定された３次元位置にロボットアーム先端を移動させるための、ロボット各関節軸の指令位置を算出する演算のことである。２８は、逆キネマティクス演算部２７からロボット各関節軸の指令位置を入力し、このロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるサーボアンプである。
【００１３】
すなわち、本発明の実施形態では３次元空間中の線速度プロファイル、線加速度プロファイル、及び線加加速度プロファイルを計算するプロファイル計算部２４と、補間点を計算する補間処理部２６との間に、線速度プロファイル等を平滑化するローパスフィルタ２５を挿入することにより、直線軌跡上で加速度を滑らかにするようにしており、係る構成において補間処理部２の後にフィルタ処理を施している図３に示した従来技術のものと相違している。
【００１４】
図６は、ローパスフィルタ２５の構成を示すブロック図である。図６において、２９（四角形のブロック）は１補間周期の遅延演算子、３０及び３２（三角形のブロック）は乗算演算子、３１（円形のブロック）は加算演算子である。遅延演算子２９は直列接続され、それぞれの出力に乗算演算子３０により各ゲインｋ₁、ｋ₂、・・・、ｋ_Nをかけて、その出力をすべて加算演算子３１にて加算する。この結果に乗算演算子３０の各ゲインｋ₁、ｋ₂、・・・、ｋ_Nの和の逆数を乗算演算子３２で乗ずる。なお、Ｎは適切に設計されたフィルタの時定数である。
【００１５】
ここでｋ₁＝ｋ₂＝・・・＝ｋ_N＝１としたとき、ローパスフィルタ２５は時定数Ｎの移動平均操作となり、ｋ₁、ｋ₂、・・・、ｋ_Nを一般的によく知られたＦＩＲフィルタ設計法で計算し与えると、所謂ＦＩＲフィルタとなる。ＦＩＲ（ＦｉｎｉｔｅＩｍｐｕｌｓｅＲｅｓｐｏｎｓｅ）とは、有限インパルス応答のことであり、ＦＩＲフィルタとは、離散化された入力信号に対して、その出力応答が有限時間長で表されるデジタルフィルタのことである。このＦＩＲフィルタには、直線位相特性を正確に実現できること、帰還回路がないので常に安定したフィルタ機能を実現できること、フーリエ級数展開等により容易に設計ができること等の利点がある。
【００１６】
図７は、ローパスフィルタ２５への入力である線速度プロファイル３３、その１階微分値（線加速度プロファイル３４）及び２階微分値（線加加速度プロファイル３５）を示すグラフである。図８は、ローパスフィルタ２５の出力である線速度プロファイル３６、その１階微分値（線加速度プロファイル３７）及び２階微分値（線加加速度プロファイル３８）を示すグラフである。このように、図８に示したローパスフィルタ２５の各出力３６、３７、３８は、図７に示したローパスフィルタ２５の各入力３３、３４、３５に対して平滑化されたものとなる。
【００１７】
これら平滑化された各プロファイル３６、３７、３８に対して補間処理部２６は補間周期毎の補間点の３次元位置を生成し、この補間周期毎の補間点の３次元位置に基づいて逆キネマティクス演算部２７はロボット各関節軸の指令位置を生成することになる。そして、このロボット各関節軸の指令位置に基づいてサーボアンプ２８はロボット各関節軸を動作させることになる。このとき、サーボアンプ２８に入力されるロボット各関節軸の指令位置は、ローパスフィルタ２５において平滑化されたデータが基になっているので、ロボットの動きは円滑なものとなる。さらに、サーボアンプ２８に入力されるロボット各関節軸の指令位置は、ロボット各関節軸の指令位置の個々に対して平滑化されたものではなく、直線軌跡開始点と直線軌跡終了点の２点間の直線軌跡上の線速度に対して平滑化されたデータが基になっているので、サーボアンプ２８の出力に基づいて動作するロボット各関節軸の動きにアンバランスが生じることはなく、よってロボットアーム先端の直線軌跡の精度が向上することになる。
【００１８】
【発明の効果】
本発明によれば、逆キネマティクス演算によりロボット各関節軸の指令位置を生成する前の段階、具体的には２点間の直線軌跡上の線速度に対してローパスフィルタ処理を行っているので、逆キネマティクス演算部で生成されたロボット各関節軸の指令位置のそれぞれに対して個々にローパスフィルタ処理を行うようにしている従来技術のように、ロボット各関節軸の動きにアンバランスが生じることはなくなった。そのため、滑らかな起動・停止を行わせることによりロボットの動きを円滑化するというフィルタ処理による効果を損なうことなく、サーボアンプに入力するロボット各関節軸の指令位置に直線軌跡ずれが生じないような産業用ロボットの制御方法及び制御装置を実現できるものとなった。その結果、本発明においては、ロボットの動きが円滑化されることによる、ロボット各関節軸に用いられている減速機や軸受などの破壊の防止と、ロボット各関節軸の指令位置に直線軌跡ずれが生じないことによる、ロボットアーム先端の直線軌跡の精度向上を両立させることが可能になった。
【図面の簡単な説明】
【図１】従来技術としてのＳ字カーブ指定法により動作経路を計画することが可能な制御装置の一例を示すブロック図である。
【図２】従来技術としての図１の制御装置における、線速度プロファイル１２、線加速度プロファイル１３、線加加速度プロファイル１４、を示すグラフである。
【図３】従来技術としての特開平１１−２４９７２４号公報に開示されている制御装置を示すブロック図である。
【図４】従来技術としての図１の制御装置における、線速度プロファイル２３を示すグラフである。
【図５】本発明の一実施形態における、制御装置を示すブロック図である。
【図６】図５で示した本発明の制御装置における、ローパスフィルタ２５の構成を示すブロック図である。
【図７】図５で示した本発明の制御装置における、ローパスフィルタ２５への入力である線速度プロファイル３３、線加速度プロファイル３４及び線加加速度プロファイル３５を示すグラフである。
【図８】図５で示した本発明の制御装置における、ローパスフィルタ２５の出力である線速度プロファイル３６、線加速度プロファイル３７及び線加加速度プロファイル３８を示すグラフである。
【符号の説明】
２４プロファイル計算部
２５ローパスフィルタ
２６逆キネマティクス演算部
２７補間処理部
２８サーボアンプ

Claims

予め指定された直線軌跡開始点と終了点とから加速度一定としたときの３次元空間中の線速度プロファイル、該線速度プロファイルの１階微分値としての線加速度プロファイル、及び前記線速度プロファイルの２階微分値としての線加加速度プロファイルを生成し、
前記線速度プロファイル、前記線加速度プロファイル、及び前記線加加速度プロファイルに対してフィルタ処理を施すことにより、平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成し、
前記平滑化された線速度プロファイル、前記平滑化された線加速度プロファイル、及び前記平滑化された線加加速度プロファイルに基づいて補間周期毎の補間点の３次元位置を生成し、
前記補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成し、
前記ロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるようにしたことを特徴とする産業用ロボットの制御方法。
予め指定された直線軌跡開始点と終了点とから加速度一定としたときの３次元空間中の線速度プロファイル、該線速度プロファイルの１階微分値としての線加速度プロファイル、及び前記線速度プロファイルの２階微分値としての線加加速度プロファイルを生成するプロファイル計算部と、
前記線速度プロファイル、前記線加速度プロファイル、及び前記線加加速度プロファイルに対してフィルタ処理を施すことにより、平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成するローパスフィルタと、
前記平滑化された線速度プロファイル、前記平滑化された線加速度プロファイル、及び前記平滑化された線加加速度プロファイルに基づいて補間周期毎の補間点の３次元位置を生成する補間処理部と、
前記補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成する逆キネマティクス演算部と、
前記ロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるサーボアンプと、
を有することを特徴とする産業用ロボットの制御装置。