(57)【要約】【課題】複数のＦＦ補償指令を一括してサーボアンプに送ることができ指令の急激な変化にも対応でき、トルク制限によるアームの落下防止ができ、ストロークの変位が可能で、作業座標系での制御ができるモーション制御システムとその制御方法を提供する。【解決手段】モーションコントローラ１は、位置指令を生成するモーション軌跡生成部７と、フィードフォワード指令演算部４０と、サーボシミュレーション部２０と、第１の通信部６０とを備え、サーボアンプ３１〜３ｎは、第２の通信部６１と、サーボ制御部１４とを備え、モーションコントローラ１とサーボアンプ３１〜３ｎは、位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を含むデータを同期して通信するようにした。【選択図】図１
2006227717.tif 000002
【特許請求の範囲】
【請求項１】
機構部を制御するモーションコントローラと、前記モーションコントローラの指令に基づき前記機構部に結合されたモータを駆動する１台以上のサーボアンプと、前記モーションコントローラと前記サーボアンプを接続する通信線とからなるモーション制御システムにおいて、
前記モーションコントローラは、
各サーボアンプへの位置指令を生成するモーション軌跡生成部と、
フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を生成するフィードフォワード指令演算部と、
前記サーボアンプと前記モータや負荷をシミュレーションするサーボシミュレーション部と、
前記サーボアンプとの通信を行う第１の通信部と、
を備え、
前記サーボアンプは、
前記モーションコントローラからの位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令に従ってモータを駆動するモータ制御部と、
前記モーションコントローラと通信を行う第２の通信部と、
を備え、
前記モーションコントローラと前記サーボアンプは、位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を含むデータを同期して通信することを特徴とするモーション制御システム。
【請求項２】
前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力または重力をフィードフォワードトルク指令で打ち消すことを特徴とする請求項１に記載のモーション制御システム。
【請求項３】
前記第１の通信部、第２の通信部は、ＩＥＥＥ１３９４シリアルバスのアイソクロナス転送を用いることを特徴とする請求項１記載のモーション制御システム。
【請求項４】
前記モーションコントローラは、前記フィードフォワードトルク指令からトルク制限値を生成するトルク制限生成部を備え、
前記サーボアンプは、前記モータ制御部の後段に配置し、トルクを制限するトルク制限部とを備え、
前記モーションコントローラと前記サーボアンプは、位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令、トルク制限値を含むデータを同期して通信することを特徴とする請求項１記載のモーション制御システム。
【請求項５】
前記トルク制限値は、予め設定された幅の固定値であることを特徴とする請求項４記載のモーション制御システム。
【請求項６】
前記トルク制限値は、サーボシミュレーション部の仮想トルク指令に応じて予め設定された幅の可変値であることを特徴とする請求項４記載のモーション制御システム。
【請求項７】
前記モーションコントローラは、前記位置指令又は前記位置応答を基に求めた座標系間の微小変位対応関係を演算し、直交座標系で設定した力もしくはトルクの限界値と前記フィードフォワードトルク指令と前記微小変位対応関係から前記トルク制限値を演算する力トルク変換部を備えることを特徴とする請求項４記載のモーション制御システム。
【請求項８】
前記モーションコントローラは、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を生成するフィードフォワード指令演算部を備え、前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力または重力による外乱をフィードフォワードトルクで打ち消す外乱補償部を備えることを特徴とする請求項４記載のモーション制御システム。
【請求項９】
前記モーションコントローラは、操作ペンダントを備え、
前記操作ペンダントは、前記トルク制限値の予め設定された幅や直交座標系で設定した力もしくはトルクの限界値を設定することを特徴とする請求項４記載のモーション制御システム。
【請求項１０】
機構部を制御するモーションコントローラと、前記モーションコントローラの指令に基づき前記機構部に結合されたモータを駆動する、１台以上のサーボアンプと、前記モーションコントローラと前記サーボアンプとを接続する通信線とからなるモーション制御システムの制御方法において、
前記モーションコントローラは、
モーション軌跡生成部で位置指令を生成し前記サーボアンプと前記サーボシミュレーション部に出力するステップと、
前記サーボシミュレーション部で位置指令からモータの仮想ＦＢ位置、仮想速度、仮想トルクを生成し、それぞれ、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令として前記第１の通信部に出力するステップと、
前記通信線は、
前記第１の通信部から前記サーボアンプの第２の通信部に位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を転送するステップと、
前記サーボアンプは、
前記モータ制御部で、前記位置指令、前記フィードフォワード位置指令、前記フィードフォワード速度指令、前記フィードフォワードトルク指令を用いてモータを制御するステップと
を備えることを特徴とするモーション制御方法。
【請求項１１】
前記フィードフォワード指令演算部は、複数のフィードフォワード補償処理を同時に処理し、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令とすることを特徴とする請求項１０記載のモーション制御方法。
【請求項１２】
前記モーションコントローラは、前記フィードフォワードトルク指令からトルク制限値を生成するステップと、
前記通信線は、前記第１の通信部から前記サーボアンプの第２の通信部に位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令と、トルク制限値を転送するステップと、
前記サーボアンプは、前記モータ制御部で、前記位置指令、前記フィードフォワード位置指令、前記フィードフォワード速度指令、前記フィードフォワードトルク指令、前記トルク制限値を用いてモータを制御するステップと、
を備えることを特徴とする請求項１０記載のモ−ション制御方法。
【請求項１３】
前記トルク制限値は、予め設定された幅の固定値であることを特徴とする請求項１２記載のモーション制御方法。
【請求項１４】
前記トルク制限値は、サーボシミュレーション部の仮想トルク指令に応じて予め設定された幅の可変値であることを特徴とする請求項１２記載のモーション制御方法。
【請求項１５】
前記モーションコントローラは、
前記位置指令又は前記位置応答を基に求めた座標系間の微小変位対応関係を演算するステップと、
直交座標系で設定した力もしくはトルクの限界値と前記フィードフォワードトルク指令と前記微小変位対応関係から前記トルク制限値を演算するステップと、
を備えることを特徴とする請求項１２記載のモーション制御方法。
【請求項１６】
前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力または重力による外乱をフィードフォワードトルクで打ち消すステップを備えることを特徴とする請求項１０記載のモーション制御方法。  
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、少なくとも１軸以上の軸数をもつモーション制御システムとその制御方法に関する。
【背景技術】
【０００２】
従来、ロボット、ＮＣなどのモータのモーション制御において、例えば特許文献１のように、その動作性能を向上させるために２自由度制御系が提案されている。これはモータや負荷のモデルを剛体系、または２慣性系で近似し、そのモデルを用いてフィードフォワード速度（以下速度ＦＦ）、フィードフォワードトルク（以下トルクＦＦ）を計算し、位置・速度フィードバックループにそのＦＦ値を印加する。またロボットなど２軸以上のアームで構成される場合、軸間に慣性力が働くので軸単位のフィードバック制御のみでは軌跡精度が低下するため、例えば特許文献２のように、非干渉化制御といった各軸の位置情報をもとにその慣性力を相殺する制御手法が提案されている。この非干渉化制御も、前記２自由度制御と同様にその補償量をトルクＦＦとして計算し、位置・速度フィードバックループに印加する。２自由度制御や非干渉化制御のように速度ＦＦやトルクＦＦを位置・速度フィードバックループに印加する形で補償する制御を、本発明ではフィードフォワード系制御と呼び、その１つの制御方式の単位をフィードフォワード補償アルゴリズムと呼ぶことにする。またフィードフォワード位置もフィードフォワード補償アルゴリズムの出力値とする。すなわちフィードフォワード補償アルゴリズムは、位置ＦＦ、速度ＦＦ、トルクＦＦの少なくとも１つを出力するアルゴリズムである。
【０００３】
また、特許文献３に示される従来のロボットは、関節軸毎の位置・速度制御系にて制御が行われていた。スポット溶接作業や組み立て作業のようにワークとの接触を伴う場合、ワーク位置が移動して、制御系に位置ずれが生し、位置ずれが過大の場合アラームになってしまう。アラームにしないためには制御剛性を高くして位置ずれを抑えなければならず、制御ゲインを高くとる必要がある。しかし、大きなトルクを発生するので逆に作業が困難になってしまう。このような問題に対して、作用力を吸収するフロート装置やＲＣＣなどの専用機械治具や、力センサを用いた力制御方式が行われていた。
また最近では、ロボットに特別な装置を付加することなく柔軟な制御を行う方式として、直交作業座標系で柔らかさを設定し、制御によって柔軟さを確保するものが開示されている。
【０００４】
またモーションコントローラと少なくとも１台以上のサーボアンプをフィールドネットワークで接続したモーション制御システムが増えている。特願２００３−２７９３５８に示されているように、フィールドネットワークのインタフェースに対応したネットワークサーボを使用するとコントローラとサーボアンプ間の配線を減らすことができ、モーションコントローラと複数のサーボアンプ間で同期した指令データとフィードバックデータを授受することができる。
【０００５】
上記フィードフォワード系制御を上記ネットワークサーボを用いたシステムに適用して、フィードフォワード補償アルゴリズムの処理をモーションコントローラに配置すると図６，７の構成となる。図６は２自由度制御を、図７は非干渉化制御を表したものである。
図６において、１００１はモーションコントローラ、１００２は通信線、１００３はサーボアンプ、１００４はモータ、１００５はＰＧ、１００６は機構部である。またモーションコントローラ１００１の内部の１００７はモーション軌跡生成部、１０４０は２自由度制御部、１０６０は通信部である。信号ａは位置指令、ｃは速度ＦＦ指令、ｄはトルクＦＦ指令である。サーボアンプ１００３の内部の１０６１は通信部、１０４４はモータ制御部であり、信号ａは位置指令、ｃは速度ＦＦ指令、ｄはトルクＦＦ指令、ｅは位置フィードバックである。
【０００６】
図７において、２００１はモーションコントローラ、２００２は通信線、２００３はサーボアンプ、２００４はモータ、２００５はＰＧ、２００６は機構部である。また、モーションコントローラ内部の２００７はモーション軌跡生成部、２０４０は非干渉制御部、２０６０は通信部であり、信号ａは位置指令、ｄはトルクＦＦ指令、ｅは位置フィードバックである。サーボアンプ２００３の内部の２０６１は通信部、２０４４はモータ制御部であり、信号ａは位置指令、ｄはトルクＦＦ指令、ｅは位置フィードバックである。
両者ともに、フィードフォワード補償アルゴリズムの処理をモーションコントローラで算出し、その補償値であるＦＦ値を通信を介してサーボアンプへ送り、サーボアンプ側ではそのＦＦ値を位置・速度フィードバックループに印加している点は共通である。異なる点は、２自由度制御の方は、入力としてモーション軌跡生成部の位置指令を使用している点、出力が速度ＦＦｃとトルクＦＦｄの２つである点、単軸に対する制御アルゴリズムであるので各々の出力が複数軸分（ｎ個）である点である。非干渉化制御の方は、入力としてサーボアンプ側から通信によって得るＦＢ位置ｅを使用している点、出力がトルクＦＦｄのみである点、複数軸に対する制御アルゴリズムであるのでトルクＦＦｄの出力が複数軸分（Ｎ個）である点である。
【特許文献１】特許第３０８４９２８号（６〜２３ページ）
【特許文献２】特開平１０−２１７１７３（２〜６ページ）
【特許文献３】特開平８−２２７３２０号公報（２〜８ページ）
【発明の開示】
【発明が解決しようとする課題】
【０００７】
ところが従来の方法では、フィードフォワード系制御すなわち非干渉化制御が使用するＦＢ位置は通信を介するので、時間遅れを含んでおり、非干渉化制御による補償機能の性能が低下するという問題があった。
また、ＦＦ系の制御は制御アルゴリズムとして独立しており、複数の制御アルゴリズムを同時に作用させることが可能であるが、従来の通信プロトコルのしくみでは位置ＦＦ，速度ＦＦ、トルクＦＦを位置指令と一括してサーボアンプへ送ることができず、さらに複数の制御アルゴリズムを同一時間内に処理し、その出力である位置ＦＦ，速度ＦＦ、トルクＦＦを各々で和をとるといった処理が整備されていないため同時に作用させることができないという問題もあった。
また、ネットワークサーボを用いたシステムで、作業座標系で柔らかい動作が可能な方式は、外界から作用するストロークが大きい場合にサーボ偏差が増大し、モータトルクも増大して作業が困難となるので、実現が不可能である。
また、通信遅れがあるため、ロボットの姿勢変化に対して実時間での演算が行えず、連続的なゲインの演算が困難なためロボットの柔らかさがロボットの姿勢により異なってしまうなどの問題点があった。
【０００８】
本発明はこのような問題点に鑑みてなされたものであり、非干渉化制御などのフィードフォワード系制御をモーションコントローラで処理しても性能低下がなく、複数のＦＦ補償指令を一括してサーボアンプに送ることができ指令の急激な変化にも対応でき、トルク制限によるアームの落下防止ができ、ストロークの変位が可能で、作業座標系での制御ができるモーション制御システムとその制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記の課題を解決するために、本発明は、次のようにしたのである。
請求項１に記載のモーション制御システムは、機構部を制御するモーションコントローラと、前記モーションコントローラの指令に基づき前記機構部に結合されたモータを駆動する１台以上のサーボアンプと、前記モーションコントローラと前記サーボアンプとを接続する通信線とからなるモーション制御システムにおいて、前記モーションコントローラは、各サーボアンプへの位置指令を生成するモーション軌跡生成部と、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を生成するフィードフォワード指令演算部と、前記サーボアンプと前記モータや負荷をシミュレーションするサーボシミュレーション部と、前記サーボアンプとの通信を行う第１の通信部と、を備え、前記サーボアンプは、前記モーションコントローラからの位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令に従ってモータを駆動するモータ制御部と、前記モーションコントローラと通信を行う第２の通信部と、を備え、前記モーションコントローラと前記サーボアンプは、位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を含むデータを同期して通信することを特徴とするものである。
請求項２に記載の発明は、請求項１に記載のモーション制御システムにおいて、前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力や重力をフィードフォワードトルク指令で打ち消すことを特徴とするものである。
請求項３に記載の発明は、請求項１に記載のモーション制御システムにおいて、前記第１の通信部、第２の通信部は、ＩＥＥＥ１３９４シリアルバスのアイソクロナス転送を用いることを特徴とするものである。
請求項４に記載の発明は、請求項１に記載のモーション制御システムにおいて、前記モーションコントローラは、前記フィードフォワードトルク指令からトルク制限値を生成するトルク制限生成部を備え、前記サーボアンプは、前記モータ制御部の最後に設置し、トルクを制限するトルク制限部とを備え、前記モーションコントローラと前記サーボアンプは、位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令、トルク制限値を含むデータを同期して通信することを特徴とするものである。
請求項５に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記トルク制限値は、予め設定された幅の固定値であることを特徴とするものである。
請求項６に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記トルク制限値は、サーボシミュレーション部の仮想トルク指令に応じて予め設定された幅の可変値であることを特徴とするものである。
請求項６に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記トルク制限値は、前記サーボシミュレーション部の仮想トルク指令に応じて予め設定された幅の可変値であることを特徴とするものである。
請求項７に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記モーションコントローラは、前記位置指令又は前記位置応答を基に求めた座標系間の微小変位対応関係を演算し、直交座標系で設定した力もしくはトルクの限界値と前記フィードフォワードトルク指令と前記微小変位対応関係から前記トルク制限値を演算する力トルク変換部を備えることを特徴とするものである。
請求項８に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記モーションコントローラは、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を生成するフィードフォワード指令演算部を備え、前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力や重力による外乱をフィードフォワードトルクで打ち消す外乱補償部を備えることを特徴とするものである。
請求項９に記載の発明は、請求項４に記載のモーション制御システムにおいて、前記モーションコントローラは、操作ペンダントを備え、前記操作ペンダントは、前記トルク制限値の予め設定された幅や直交座標系で設定した力もしくはトルクの限界値を設定することを特徴とするものである。
【００１０】
請求項１０に記載の発明は、機構部を制御するモーションコントローラと、前記モーションコントローラの指令に基づき前記機構部に結合されたモータを駆動する、１台以上のサーボアンプと、前記モーションコントローラと前記サーボアンプとを接続する通信線とからなるモーション制御システムの制御方法において、前記モーションコントローラは、
モーション軌跡生成部で位置指令を生成し前記サーボアンプと前記サーボシミュレーション部に出力するステップと、前記サーボシミュレーション部で位置指令からモータの仮想ＦＢ位置、仮想速度、仮想トルクを生成し、それぞれ、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令として前記第１の通信部に出力するステップと、前記通信線は、前記第１の通信部から前記サーボアンプの第２の通信部に位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令を転送するステップと、前記サーボアンプは、前記モータ制御部で、前記位置指令、前記位置フィードフォワード指令、前記フィードフォワード速度指令、前記フィードフォワードトルク指令を用いてモータを制御する（する）ステップとを備えることを特徴とするものである。
請求項１１に記載の発明は、請求項１０に記載のモーション制御方法において、前記フィードフォワード指令演算部は、複数のフィードフォワード補償処理を同時に処理し、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令とすることを特徴とするものである。
請求項１２に記載の発明は、請求項１０に記載のモーション制御方法において、前記モーションコントローラは、前記フィードフォワードトルク指令からトルク制限値を生成するステップと、前記通信線は、前記第１の通信部から前記サーボアンプの第２の通信部に位置指令、フィードフォワード位置指令、フィードフォワード速度指令、フィードフォワードトルク指令と、トルク制限値を転送するステップと、前記サーボアンプは、前記モータ制御部で、前記位置指令、前記位置フィードフォワード指令、前記フィードフォワード速度指令、前記フィードフォワードトルク指令、前記トルク制限値を用いてモータを制御するステップと、を備えることを特徴とするものである。
請求項１３に記載の発明は、請求項１２に記載のモーション制御方法において、前記トルク制限値は、予め設定された幅の固定値であることを特徴とするものである。
請求項１４に記載の発明は、請求項１２に記載のモーション制御方法において、前記トルク制限値は、前記サーボシミュレーション部の仮想トルク指令に応じて予め設定された幅の可変値であることを特徴とするものである。
請求項１５に記載の発明は、請求項１２に記載のモーション制御方法において、前記モーションコントローラは、前記位置指令又は前記位置応答を基に求めた座標系間の微小変位対応関係を演算するステップと、直交座標系で設定した力もしくはトルクの限界値と前記フィードフォワードトルク指令と前記微小変位対応関係から前記トルク制限値を演算するステップと、を備えることを特徴とすものである。
請求項１６に記載の発明は、請求項１２に記載のモーション制御方法において、前記フィードフォワード指令演算部は、２軸以上のアームから構成されるロボットの軸間に発生する慣性力または重力による外乱をフィードフォワードトルクで打ち消すステップを備えることを特徴とするものである。請求項１０記載のモーション制御システム。
【発明の効果】
【００１１】
請求項１、３に記載の発明によれば、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れのない仮想ＦＢ位置を計算させ、フィードフォワード系制御で使用することができるので、補償機能の性能低下を抑制するモーション制御システムを提供できる。
また請求項２に記載の発明によれば、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れのない仮想ＦＢ位置を計算させ、これを非干渉化制御やむ外乱補償制御で使用することができるので、その補償機能の性能低下を抑制するモーション制御システムを提供できる。
また請求項４乃至６に記載の発明によれば、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れ分を考慮した仮想トルク指令を計算させ、これに応じてトルク制限値を各サーボアンプに与えることができるので、指令の急激な変化にも対応するモーション制御システムを提供できる。
また請求項７に記載の発明によれば、力トルク変換部といったソフトウェアモジュールをモーションコントローラ部に設け、直交座標系で設定した力もしくはトルクの限界値とフィードフォワードトルク指令と微小変位対応関係から関節座標系のトルク制限値を演算することで、指令の急激な変化にも対応した直交座標系の軸毎に力制限を行うモーション制御システムを提供できる。
また請求項８に記載の発明によれば、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れのない仮想ＦＢ位置を計算させ、これを外乱補償制御で使用することができるので、トルク制限によるアームの落下を防止するモーション制御システムを提供できる。
また請求項９に記載の発明によれば、モーションコントローラ部に接続された操作ペンダントから、前記トルク制限値の予め設定された幅や直交座標系で設定した力もしくはトルクの限界値を入力してトルクを制限し、現場で作業者が作業に応じて直感的に条件を変更可能なモーション制御システムを提供できる。
【００１２】
また請求項１０乃至１２に記載の発明によると、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れのない仮想ＦＢ位置を計算させ、フィードフォワード系制御で使用することができるので、補償機能の性能低下を抑制するモーション制御方法を提供できる。
また請求項１３に記載の発明によれば、フィードフォワード系制御を同時に複数実行できるしくみとして、その出力であるＦＦ値を畳重するしくみと、位置ＦＦ，速度ＦＦ、トルクＦＦを位置指令と同時にサーボアンプへ送信できるプロトコルを実現することができるので、複数のフィードフォワード補償アルゴリズムをモーションコントローラ側で同時に作用させることを可能にでき補償機能の性能低下を抑制するモーション制御方法を提供できる。
また請求項１４に記載の発明によれば、力トルク変換部といったソフトウェアモジュールをモーションコントローラ部に設け、直交座標系で設定した力もしくはトルクの限界値とフィードフォワードトルク指令と微小変位対応関係から関節座標系のトルク制限値を演算することで、指令の急激な変化にも対応した直交座標系の軸毎に力制限を行うモーション制御方法を提供できる。
また請求項１５に記載の発明によれば、サーボアンプシミュレーション部といったソフトウェアモジュールをモーションコントローラ部に設け、サーボドライブ応答をソフトウェアでシミュレーションし、位置指令に対して時間遅れのない仮想ＦＢ位置を計算させ、これを外乱補償制御で使用することができるので、トルク制限によるアームの落下を防止するモーション制御方法を提供できる。
また請求項１６に記載の発明によれば、モーションコントローラ部に接続された操作ペンダントから、前記トルク制限値の予め設定された幅や直交座標系で設定した力もしくはトルクの限界値を入力できるので、現場で作業者が作業に応じて直感的に条件を変更できるモーション制御方法を提供できる。
【発明を実施するための最良の形態】
【００１３】
以下、本発明の方法の具体的実施例について、図に基づいて説明する。
【実施例１】
【００１４】
まず、モーションコントローラ内のデータの流れについて図１を用いて説明する。モーション軌跡生成部７は、各サーボアンプ３へ指令する位置指令８を生成する。具体的には制御対象が工作機械であれば直線補間や円弧補間された位置指令８であり、ロボットであれば直交空間で補間したデータを逆キネマティクス変換して各軸の関節角に計算された位置指令８である。位置指令８はサーボドライブシミュレーション部２０と通信部６０へ渡される。サーボドライブシミュレーション部２０は、入力された位置指令８に基づいて、モーションコントローラ１に接続されているサーボアンプ３１からサーボアンプ３ｎのｎ個のサーボのシミュレーション計算を行い、仮想ＦＢ位置ｅをフィードフォワード指令演算部４０へ出力する。サーボドライブシミュレーション部２０のモデルの詳細については後述する。フィードフォワード指令演算部４０は、入力された仮想ＦＢ位置ｅを使用してフィードフォワード系の補償演算を行い、その結果である位置ＦＦａ、速度ＦＦｃ、トルクＦＦｄを第１の通信部６０へ出力する。フィードフォワード指令演算部４０の詳細については後述する。通信部６０は、モーション軌跡生成部７からの位置指令ａと、フィードフォワード指令演算部４０からの位置ＦＦｂ，速度ＦＦｃ，トルクＦＦｄの４つのデータを含むデータを軸単位でまとめ、同期通信機能を使ってサーボアンプ３へ送信する。
【００１５】
次に同期通信について説明する。ＩＥＥＥ１３９４ネットワークのアイソクロナス通信を用いたモーション制御システム向けのマスター・スレーブ同期通信方式が提案されている（特許文献３を参照）。特許文献３の図１において、本システムの通信線をＩＥＥＥ１３９４のシリアル伝送路に、モーションコントローラを主局に、サーボアンプを従局とすると、モーションコントローラとサーボアンプ間で同期通信を実現することができる。
【００１６】
次にサーボアンプ３について図２を用いて説明する。図２は典型的な位置・速度フィードバックループの基本制御ブロック図である。第２の通信部６１に届いた位置指令ａ、位置ＦＦ指令ｂ、速度ＦＦ指令ｃ、トルクＦＦ指令ｄは、それぞれ，位置制御ブロック、速度制御ブロック、トルク制御ブロックの指令にフィードフォワードする形で印加する。こうすることにより、従来の位置・速度フィードバックループの機能を維持したまま、フィードフォワードの機能を追加することができる。
【００１７】
次にサーボドライブシミュレーション部２０について図３を用いて説明する。図３はモータ４と負荷が剛体モデルである場合の位置・速度フィードバックループの制御ブロック図を表している。本発明における本サーボシミュレーション部２０の目的は、フィードフォワード指令演算部４０が使用するモータ４の位置をシミュレーションによって通信遅れの影響を除去したデータとして提供することである。本主旨に照らし合わせると、サーボアンプ３の制御モデルとモータ４のモデルと機構部６のモデルを図３のモデルで近似しても、十分にその効果がある。またサーボシミュレーション部２０は、本システムに接続されているサーボアンプ３の数だけモデルを持ち、演算周期毎に全モデルの計算を行う。図において、Ｋｐは位置ループゲイン、Ｋｖは速度制御ゲイン、Ｔｉは速度制御積分時定数、Ｊはモータ４と負荷のイナーシャの合算値である。Ｋｐ、Ｋｖ、Ｔｉはサーボアンプ３で使用している値を、Ｊについてはモータイナーシャはカタログ値、負荷イナーシャは設計値を使用すればよい。
【００１８】
次にフィードフォワード指令演算部４０について図４と５を用いて説明する。フィードフォワード指令演算部４０にフィードフォワード補償アルゴリズムは複数存在してよい。モーションコントローラ１は、フィードフォワード指令演算部４０に登録されている全てのフィードフォワード補償アルゴリズムを演算周期毎に実行する。フィードフォワード補償アルゴリズムはサーボドライブシミュレーション部２０が出力した仮想ＦＢ位置ｅを使用してもしなくてもよい。フィードフォワード補償アルゴリズムは、位置ＦＦｂ、速度ＦＦｃ、トルクＦＦｄの少なくとも１つのＦＦ値を計算して出力する。３つのＦＦ値のうち補償する必要のないものには０をセットする。フィードフォワード補償アルゴリズムは、１つ以上のサーボアンプ３に対するＦＦ値を出力できる。例えばｎ個の自由度（モータ４）からなるロボットにおける非干渉化制御の場合、位置ＦＦｂと速度ＦＦｃは使用しないが、トルクＦＦｄはｎ個分の出力となる。図４は、フィードフォワード補償アルゴリズムがｎ個、サーボアンプ３は１個で、ｎ個全てのアルゴリズムが位置ＦＦｂ１１、ｂ２１、・・・、ｂｎ１、速度ＦＦｃ１１、ｃ２１、・・・、ｃｎ１、トルクＦＦｄ１１、ｄ２１、・・・、ｄｎ１を出力して、それらが各々合算されｂ１１＋ｂ２１＋・・・＋ｂｎ１＝ｂ１、ｃ１１＋ｃ２１＋・・・＋ｃｎ１＝ｃ１、ｄ１１＋ｄ２１＋・・・＋ｄｎ１＝ｄ１となる動きを図示したものである。一方、図５は、フィードフォワード補償アルゴリズムが３個、サーボアンプ３は２個で、アルゴリズム１は１軸目に対する位置ＦＦｂ１１のみ、アルゴリズム２は２軸目に対する位置ＦＦｂ２２のみ、アルゴリズム３は１軸目と２軸目両方に対する位置ＦＦｂ３１とｂ３２を出力して、それらが各々合算されｂ１１＋ｂ３１＝ｂ１、ｂ２２＋ｂ３２＝ｂ２、ｂ３１＋ｂ３２＝ｂ３となる動きを図示したものである。こうして計算された各サーボアンプ３に対する位置ＦＦ，速度ＦＦ，トルクＦＦは通信部６０へ渡され、次回の通信でサーボアンプ３へ送信される。
【００１９】
次にフィードフォワード補償アルゴリズムの例として、ロボットにおける非干渉化制御を説明する。アルゴリズムの詳細に関しては、特許文献２に記載されている通り、本アルゴリズムは軸間に発生する慣性力を除去する制御アルゴリズムであるが、その演算周期の計算で必要とされる値（特許文献２では非干渉化用状態変数Ｘｉｋ）は、アーム加速度、その１回微分値、２回微分値、積分値の４変数である。本システムにおいては、このアーム加速度を該当関節（モータ４）の仮想ＦＢ位置ｅの差分から求めることで代用する。重力補償制御も同様にして実現できる。
【００２０】
次に、トルク制限による柔軟制御を例にして、ロボットにおけるスポット溶接作業時の倣い機能を図８及び図９を用いて説明する。
まず、通信遅れを考慮しないで、位置指令を２階微分した加速度指令に応じてトルク制限を可変にした場合の様子を図９（ａ）に示す。図９（ａ）では、加減速部分でトルク指令がトルク制限にかかり、動作が不安定になってしまうという問題がある。
そこで、モーションコントローラ１において、サーボドライブシミュレーション部２０で予め指令の先読みなどで通信遅れを考慮した仮想トルク指令７１を演算し、トルク制限演算部７４内でサーボアンプ３に送るトルク制限値７２を演算する。トルク制限値７２の演算の方法としては、予め作業に応じて加減速トルク分よりも大きく設定した固定値を用いる方法や、図９（ｂ）に示すように通信遅れ分を考慮した仮想トルク指令７１の上下にある幅αを持たせた可変のトルク制限値７２にする方法がある。これらのトルク制限値７２をサーボアンプ３に送り、モータ制御部１４の後段に設けられたトルク制限部７３に設定する。外部から力が加わった場合でも、サーボアンプ３内のモータ制御部１４内のトルク指令が制限されることで、外力に対して過大な力を発生することなく、倣うことができ、上位からの動作指令がある場合でもトルク制限にかかることなく、動作することができる。
これにより、例えば、ワークのセット誤差や加工誤差がある場合でも、ロボットの先端に付いたスポット溶接ガンがワークに接触しても、外力に対して倣うことで作業を続行できる。
【００２１】
次に、直交座標系での力制限による柔軟制御を例にして、ロボットにおけるスポット溶接作業時の倣い機能を図１０及び図１１を用いて説明する。
力トルク変換部７５において、前記位置指令８から関節座標と直交座標の座標系間の微小変位対応関係（一般的にヤコビ行列とも言う）を演算する。ここで、前記位置指令８の代わりに位置応答１０を用いても良い。
次に、サーボドライブシミュレーション部２０の内部状態量である仮想トルク指令７１を前記微小変位対応関係の逆行列を用いて直交座標系の仮想力指令に変換し、この仮想力指令に対して、予め直交座標系で設定した力もしくはトルクの限界値７７を加算し、上下にある幅を持たせた可変の力制限値を求め、この可変力制限値を、前記微小変位対応関係の転置行列で関節座標系のフィードフォワードトルク制限値に変換する。ここで、仮想力指令を用いずに、予め作業に応じて大きく設定された直交座標系で設定した力もしくはトルクの限界値７７を直接前記微小変位対応関係で関節座標系のトルク制限値に変換しても良い。
前記サーボアンプ３は、モーションコントローラ１からのトルク制限値に従って、モータ制御部１４の後段に設けたトルク制限部７３で、トルク指令を制限しつつ、モータを駆動する。
また、フィードフォワード指令演算部４０内でアームの重力トルク７６を演算し、トルク制限部７３の後段でトルク指令に加算することで、自重を支えるためのトルクをモータ制御部１４内で補償する必要がなくなり、トルク制限値をより小さく設定することができる。
次に、図１１に示すように、スポット溶接の作業を例にして、作業条件の設定を説明する。ここでは、スポット溶接ガン９１の開閉方向（ここではツール座標系Ｔ−ｘｙｚのＺ方向）とワーク９２の座標系の力制限を低く設定した軸方向（ここではＷ−ｘｙｚのＺ方向）を一致させ、開閉方向以外の力制限を高く設定して、スポット溶接ガン９１の開閉方向にのみロボット９０が柔軟に倣い、開閉方向以外には位置ズレしない倣い制御を用いる。これにより、開閉方向のワークズレや加工誤差に対応して倣い制御しつつ、横方向の教示位置に対しては精度良く位置決めを行うことができる。また、予め上位からの動作指令によるトルク分は考慮されており、通信遅れ分も補償されているため、急激に指令が変化する場合でもトルク制限にかかることなく動作可能である。
作業条件はコントローラ９３に接続された操作ペンダント９４の画面上で行う。図１２に操作画面を示す。座標系の選択をスポット溶接ガン９１上に設定されたツール座標系を選択し、ツール座標系Ｔ−ｘｙｚのＺ方向の力制限を小さく設定し、ＸＹ方向の力制限を大きく設定する。このように柔軟な方向を直感的に理解できるため、ロボットの姿勢などを考慮せず誰でも設定できる。
【００２２】
図１３はモーション制御システムの制御方法を示したフローチャートである。図１３において、ステップＳＴ１は、モーションコントローラが位置指令を生成するステップである。次に、ステップＳＴ２では、位置指令からシミュレーションにより仮想のトルク、仮想の速度、仮想の位置を生成する。次にステップＳＴ３では、モーションコントローラからサーボアンプに指令が転送される。次にステップＳＴ４では、サーボアンプが、位置指令、位置フィードフォワード指令、速度フィードフォワード指令、トルクフィードフォワード指令を指令としてモータを制御する。
【産業上の利用可能性】
【００２３】
本発明によると、非干渉化制御などのフィードフォワード系制御をモーションコントローラで処理しても性能低下のないモーション制御システムとその制御方法を提供できるので、ロボットだけでなく軸が干渉するような機械の制御には適用が期待できる。
【図面の簡単な説明】
【００２４】
【図１】本発明のモーション制御システムの構成を示すブロック図
【図２】本発明のサーボアンプの構成を示すブロック図
【図３】本発明のサーボドライブシミュレーション部の制御ブロック図
【図４】本発明のフィードフォワード指令演算部構成を示す図１
【図５】本発明のフィードフォワード指令演算部構成を示す図２
【図６】従来の２自由度制御のモーション制御システムの構成を示すブロック図
【図７】従来の非干渉化制御のモーション制御システムの構成を示すブロック図
【図８】本発明のモーション制御システムの構成を示すブロック図２
【図９】本発明のトルク制限の状態を示す図
【図１０】本発明のモーション制御システムの構成を示すブロック図３
【図１１】本発明の力制限の作業例を示す図
【図１２】本発明の力制限の作業条件例を示す図
【図１３】モーション制御システムの制御方法を示すフローチャート
【符号の説明】
【００２５】
１  モーションコントローラ
２  通信線
３１、・・、３ｎ  サーボアンプ
４１〜４ｎ  モータ
５１〜５ｎ  ＰＧ
６１〜６ｎ  機構部
７  モーション軌跡生成部
２０  サーボシミュレーション部
４０  フィードフォワード指令補償部
６０  第１の通信部
６１１〜６１ｎ  第２の通信部
７１  仮想トルク指令
７２  トルク制限値
７３  トルク制限部
７４  トルク制限演算部
７５  力トルク変換部
７６  重力トルク
７７  直交座標系で設定した力もしくはトルクの限界値
９０  ロボット
９１  スポット溶接ガン
９２  ワーク
９３  コントローラ
９４  操作ペンダント
１４１〜１４ｎ  モータ制御部  位置ＦＦ指令
ａ、ａ１〜ａｎ、ａ１１〜ａｎｎ  位置指令
ｂ、ｂ１〜ｂｎ、ｂ１１〜ｂｎｎ  位置フィードフォワード指令
ｃ、ｃ１〜ｃｎ、ｃ１１〜ｃｎｎ  速度フィードフォワード指令
ｄ、ｄ１〜ｄｎ、ｄ１１〜ｄｎｎ  トルクフィードフォワード指令
１４、１４１〜１４ｎ  モータ制御部
４０１  フィードフォワード補償アルゴリズム１
４０２  フィードフォワード補償アルゴリズム２
４０３  フィードフォワード補償アルゴリズム３
【図１】
2006227717.tif 000003
【図２】
2006227717.tif 000004
【図３】
2006227717.tif 000005
【図４】
2006227717.tif 000006
【図５】
2006227717.tif 000007
【図６】
2006227717.tif 000008
【図７】
2006227717.tif 000009
【図８】
2006227717.tif 000010
【図９】
2006227717.tif 000011
【図１０】
2006227717.tif 000012
【図１１】
2006227717.tif 000013
【図１２】
2006227717.tif 000014
【図１３】
2006227717.tif 000015