(57)【要約】【課題】関節軸が反転を行う際のエンドエフェクタの速度変動の制御性能の劣化を防止するロボット制御装置及びロボットの制御方法を提供する。【解決手段】コントローラＲＣは複数の教示点に基づいて目標軌道を作成するＣＰＵ９１を備え、ＣＰＵ９１は、目標軌道に基づく関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する。ＣＰＵ９１は、選択した関節軸のサーボモータ４１〜４７の反転のための減速期間中は、速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算し、減速期間経過後は速度制御ループの積分ゲインを大きくし、所定期間経過後に積分ゲインを元の大きさに戻す。【選択図】図２
2014159066.tif 000002
【特許請求の範囲】
【請求項１】
多関節ロボットの関節軸を駆動するサーボモータを、比例制御及び積分制御を備えた速度制御ループを含む制御部によりサーボ制御するロボット制御装置において、
複数の教示点に基づいて目標軌道を作成する目標軌道作成部と、
前記目標軌道に基づく前記関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する選択部とを備え、
前記制御部は、前記選択した関節軸のサーボモータの反転のための減速期間中は、前記速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算し、
前記減速期間経過後は前記速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻すことを行うロボット制御装置。
【請求項２】
前記多関節ロボットが冗長ロボットであり、
前記選択部は、拘束条件で固定した関節軸を除いた残りの関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択するものである請求項１に記載のロボット制御装置。
【請求項３】
多関節ロボットの関節軸を駆動するサーボモータを、比例制御及び積分制御を備えた速度制御ループを含む制御部によりサーボ制御するロボットの制御方法において、
複数の教示点に基づいて目標軌道を作成する目標軌道作成段階と、
前記目標軌道に基づく前記関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する選択段階と、
前記選択した関節軸のサーボモータの反転のための減速期間中は、前記速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算する段階と、
前記減速期間経過後は前記速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻す段階を含むロボットの制御方法。
【請求項４】
前記多関節ロボットが冗長ロボットであり、
前記選択段階は、拘束条件で固定した関節軸を除いた残りの関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択するものである請求項３に記載のロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置及びロボットの制御方法に関する。
【背景技術】
【０００２】
従来、多関節ロボットを動作させる場合、動作する関節軸が、正転から逆転、または逆転から正転のように反転すると、サーボモータに連結されているギヤ等のバックラッシュによる無制御領域が発生し、エンドエフェクタの速度変動等の制御性能が著しく劣化する問題がある。
【０００３】
このような問題点に対して、例えば、特許文献１の技術が公知である。特許文献１では、工作機械のテーブル等の送り軸や、ロボットのアーム等を駆動するサーボモータの制御において、当該周期の位置指令と次の周期の位置指令の方向が反転する時の加速度を求め、該加速度に比例した値（すなわち、機械の摩擦力によって決まる値）と速度指令が一致した時点でバックラッシュ加速補正を行うようにしている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平５−１１８２４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１では、バックラッシュ加速補正を行う場合に、加速度に比例した値（すなわち、機械の摩擦力によって決まる値）と速度指令が一致した時点でバックラッシュ加速補正を行うようにしているため、制御遅れがあり、効果的ではない。
【０００６】
本発明の目的は、関節軸が反転を行う際のエンドエフェクタの速度変動の制御性能の劣化を防止するロボット制御装置及びロボットの制御方法を提供することにある。
【課題を解決するための手段】
【０００７】
上記問題点を解決するために、本ロボット制御装置は、多関節ロボットの関節軸を駆動するサーボモータを、比例制御及び積分制御を備えた速度制御ループを含む制御部によりサーボ制御するロボット制御装置において、複数の教示点に基づいて目標軌道を作成する目標軌道作成部と、前記目標軌道に基づく前記関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する選択部とを備え、前記制御部は、前記選択した関節軸のサーボモータの反転のための減速期間中は、前記速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算し、前記減速期間経過後は前記速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻すことを行うものである。
【０００８】
前記多関節ロボットが冗長ロボットの場合、前記選択部は、拘束条件で固定した関節軸を除いた残りの関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択することが好ましい。
【０００９】
また、本ロボットの制御方法は、多関節ロボットの関節軸を駆動するサーボモータを、比例制御及び積分制御を備えた速度制御ループを含む制御部によりサーボ制御するロボットの制御方法において、複数の教示点に基づいて目標軌道を作成する目標軌道作成段階と、前記目標軌道に基づく前記関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する選択段階と、前記選択した関節軸のサーボモータの反転のための減速期間中は、前記速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算する段階と、前記減速期間経過後は前記速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻す段階を含むものである。
【００１０】
前記多関節ロボットが冗長ロボットの場合、前記選択段階は、拘束条件で固定した関節軸を除いた残りの関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択することが好ましい。
【発明の効果】
【００１１】
本発明のロボット制御装置及びロボットの制御方法によれば、関節軸のサーボモータの反転のための減速期間中は速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算するため、減速を早期に完了することができる。また、本発明によれば、減速期間経過後のギヤのバックラッシュ期間では負荷側のイナーシャがなくなるが、このバックラッシュ期間では速度制御ループの積分ゲインを大きくするため、バックラッシュ期間を通過後の加速開始を早期に行うことができる。従って、本発明のロボット制御装置及びロボットの制御方法によれば、関節軸が反転を行う際のエンドエフェクタの速度変動の制御性能の劣化を防止することができる。
【図面の簡単な説明】
【００１２】
【図１】一実施形態のマニピュレータのスケルトン図。
【図２】一実施形態のロボット制御装置の概略構成図。
【図３】一実施形態のロボット制御装置が実行するプログラムのフローチャート。
【図４】一実施形態のロボット制御装置が実行するプログラムのフローチャート。
【図５】制御部の位置制御ループ、速度制御ループ、電流制御ループのブロック線図。
【図６】正転及び逆転時の定速期間、減速期間、加速期間の説明図。
【発明を実施するための形態】
【００１３】
以下、本発明を具体化した一実施形態の７軸マニピュレータを制御するロボット制御装置及びロボットの制御方法を図１〜図６を参照して説明する。
まず、本実施形態の作業自由度に対して冗長自由度を有するマニピュレータについて説明する。
【００１４】
図１に示すように、マニピュレータ１０は、８個のリンク１１〜１８が７個の関節２１〜２７により直列に連結されて形成されている。多関節ロボットであるマニピュレータ１０は、７個の関節２１〜２７においてリンク１２〜１８が旋回することのできる７自由度（自由度ｎ＝７）を有するロボットであり、その作業空間の次元数（次元数ｍ）は６であって、１（＝ｎ−ｍ）の冗長性を有する。
【００１５】
第１リンク１１は一端が床面ＦＬに固定され、他端が第１関節２１の一側に接続されている。第１関節２１の他側には、第２リンク１２の一端が接続され、第２リンク１２の他端には第２関節２２の一側が接続されている。以下同様に、第３リンク１３、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８が、それぞれ第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７を介して順に連結されている。
【００１６】
第１関節２１の他側は一側に対して、矢印３１に示すように、図１において上下方向に延びる軸を中心に回転可能とされており、これにより、第２リンク１２は隣接する第１リンク１１に対して、第１関節２１の回転軸（Ｊ１軸）を中心に矢印３１方向に旋回可能である。
【００１７】
また、第２関節２２の他側は一側に対して、矢印３２に示すように、図１において紙面に垂直な方向に延びる軸（Ｊ２軸）を中心に回転可能とされている。これにより、第３リンク１３は隣接する第２リンク１２に対して、第２関節２２の回転軸を中心に矢印３２方向、すなわち、上下方向に回転可能である。
【００１８】
以下、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７についてもそれぞれ、回転可能とされており、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８も、それぞれ関節２３〜２７の回転軸（Ｊ３軸〜Ｊ７軸）を中心に、矢印３３〜３７方向に旋回可能である。尚、本願の全体にわたって、第１関節２１〜２７を介して連結されているリンク１１〜１８同士を、互いに隣接するリンク１１〜１８という。また、Ｊ１軸〜Ｊ７軸は、関節軸に相当する。
【００１９】
なお、Ｊ２軸、Ｊ４軸及びＪ６軸の回転方向は、図１に示すように、重力加速度が働く方向と一致している。
図１に示すように、第１関節２１には第１サーボモータ４１が取り付けられており、電力が供給されることにより、第２リンク１２を図示しない減速機を介して第１リンク１１に対して旋回させる。
【００２０】
また、第２関節２２には第２サーボモータ４２が取り付けられており、電力が供給されることにより、第３リンク１３を図示しない減速機を介して第２リンク１２に対して旋回させる。以下、同様に、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７にはそれぞれサーボモータ４３〜４７が取り付けられており、電力が供給されることにより、各々リンク１４〜１８を図示しない減速機を介して旋回させる。
【００２１】
なお、各モータは、各関節内に設けられるが、図１では、説明の便宜上、関節とは分離して図示している。また、本実施形態では回転系アクチュエータ（以下、単にアクチュエータという）としてサーボモータであるＡＣモータが使用されているが、限定されるものではない。
【００２２】
第８リンク１８の先端には、エンドエフェクタとしてのツール４９が取り付けられている。ツール４９は第８リンク１８とともに、第７関節２７の回転軸（Ｊ７軸）を中心に図１に示すように矢印３７方向に旋回可能とされている。ツール４９は、例えば、ワーク等を把持可能なハンドである。なお、ツール４９の種類は、本発明とは関係しないため、限定されるものではない。
【００２３】
上述したようにマニピュレータ１０は、第１サーボモータ４１〜第７サーボモータ４７を駆動して第２リンク１２〜第８リンク１８を回転させることにより、第２リンク１２〜第８リンク１８の回転角度が累積して先端部にあるツール４９に働くため、ツール４９の先端の位置および姿勢を、その作業内容に応じた目標位置および目標姿勢に一致させることが可能である。
【００２４】
次に、図２を参照して、前記マニピュレータ１０を制御するロボット制御装置としてのコントローラＲＣを中心とした多関節ロボットの電気的な構成を説明する。
コントローラＲＣは、コンピュータ９０と、コンピュータ９０に電気的に接続されたＰＷＭジェネレータ５１〜５７と、ＰＷＭジェネレータ５１〜５７に電気的に接続されたサーボアンプ６１〜６７を有する。各サーボアンプ６１〜６７はそれぞれ第１サーボモータ４１〜第７サーボモータ４７に電気的に接続されている。
【００２５】
コンピュータ９０は、制御指令をＰＷＭジェネレータ５１〜５７に出力し、ＰＷＭジェネレータ５１〜５７は、当該制御指令に基づいてＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じてサーボモータ４１〜４７を作動させることにより、各リンク１２〜１８を回転させる。
【００２６】
前記サーボモータ４１〜４７にはロータリエンコーダ７１〜７７が内蔵されており、インターフェイス８０を介してコンピュータ９０と接続されている。ロータリエンコーダ７１〜７７は、各々のサーボモータ４１〜４７の回動角度を検出することにより、すなわち、リンク１２〜１８のそれぞれが隣接するリンク１１〜１７に対する回転角度（なお、関節軸の関節角度ということがある）を検出して、その検出信号をコントローラＲＣに送信する。ロータリエンコーダ７１〜７７は、回転角度検出器に相当する。なお、回転角度検出器としては、ロータリエンコーダに限定するものではなく、レゾルバ、或いは、ポテンショメータであってもよい。
【００２７】
なお、第１サーボモータ４１〜第７サーボモータ４７に対してロータリエンコーダ７１〜７７を設ける代わりに、リンク１１〜１８または第１関節２１〜第７関節２７に、リンク１１〜１８の回転角度（関節軸の関節角度）を直接に検出可能なセンサを取り付けてもよい。
【００２８】
前記コンピュータ９０は、ＣＰＵ９１、ＲＯＭ９２、ＲＡＭ９３、及びハードディスク等の不揮発性の記憶部９４、及びインターフェイス９５等を備え、バス９６を介して電気的に接続されている。
【００２９】
記憶部９４には、各種データ、ロボットに各種作業を行わせるための作業プログラム、各種パラメータ等が記憶されている。作業プログラムは、教示ステップ（教示により記述されたプログラムの行）毎に、教示点、動作指令或いは各種動作条件（例えば、移動速度を含む）等のパラメータが記述されている。
【００３０】
本実施形態のロボットは、ティーチングプレイバック方式で作動するロボットであり、前記作業プログラムが実行されることにより、前記マニピュレータ１０が動作する。ＲＯＭ９２は、システム全体のシステムプログラムが記憶されている。ＲＡＭ９３は、ＣＰＵ９１の作業用のメモリであって、各種演算等が実行されるときに一時的にデータが格納される。ＣＰＵ９１は、目標軌道作成部、選択部、及び制御部に相当する。
【００３１】
コントローラＲＣには入力装置８２が前記インターフェイス９５を介して接続されている。入力装置８２は、図示しないモニター画面及び各種入力キー等を有する操作盤であり、ユーザーが各種のデータを入力操作可能とされている。入力装置８２は、多関節ロボットの電源スイッチが設けられるとともに、コンピュータ９０に対して、マニピュレータ１０の先端部にあるツール４９の先端（以下、手先という）の最終目標位置および最終目標姿勢、ツール４９の先端の補間点における位置および姿勢の入力、並びに、冗長性を利用したマニピュレータ１０の姿勢変更のためのジョグ操作等による入力が可能となっている。
【００３２】
（実施形態の作用）
次に、本実施形態による多関節ロボットのコントローラＲＣにおける作業プログラム再生のフローチャートを図３及び図４を参照して説明する。
【００３３】
（Ｓ１０）
ＣＰＵ９１は、まず記憶部９４から作業プログラムのプログラム行を読込みする。
（Ｓ２０）
Ｓ２０では、ＣＰＵ９１は、プログラム行、すなわち、動作指令、教示点、姿勢が記述された行があるか否かを判定する。なければ、この作業プログラム再生を終了する。動作指令及び教示点が記述された行があればＳ３０に移行する。
【００３４】
（Ｓ３０）
ＣＰＵ９１は、プログラム行に記述された行の軌道計画を行う。すなわち、ＣＰＵ９１は、前記プログラム行に記述されている教示点及び姿勢に基づいて目標軌道を設定する。また、ＣＰＵ９１は、図示しない補間周期カウンタをゼロに設定する。Ｓ３０は、目標軌道作成段階に相当する。
【００３５】
（Ｓ４０）
Ｓ４０では、ＣＰＵ９１は補間が終了したか否かを判定する。
すなわち、前記補間周期カウンタと軌道計画で定められた補間点数とを比較し、補間周期カウンタが補間点数以上ならば、その行の動作は終了したことになり、処理はＳ１０に戻って次に選択される行を実行する。一方、補間周期カウンタが補間点数未満の場合は、その行の動作は終了しておらず、次のＳ５０の処理に移行する。
【００３６】
（Ｓ５０）
Ｓ５０では、ＣＰＵ９１は、動作補間を行う。ＣＰＵ９１は、前記軌道計画に基づいて、補間周期毎のツール４９（エンドエフェクタ）の目標位置を作成する。
【００３７】
なお、軌道計画の処理周期は、後述の補間周期よりも長くしている。
（Ｓ６０）
Ｓ６０では、ＣＰＵ９１は、前記目標軌道上の各点（なお、各点には教示点及び補間点を含む）における手先位置姿勢から関節角度を求めるために逆変換演算を行う。
【００３８】
ここで、各点における第１関節２１〜第７関節２７の関節角度θ１，θ２，θ３，…，θ７とし、手先座標（ｘ，ｙ，ｚ）及び手先姿勢（ａ，ｂ，ｃ）とすると、ベクトルΘ及び手先位置姿勢Ｘは下記のように表される。
【００３９】
【数１】
2014159066.tif 000003
各関節角度θ１，θ２，θ３，…，θ７は、式（２）に示すように表すことができ、これらの式は、逆変換式である。本ステップでは、式（２）により、手先位置姿勢Ｘと姿勢パラメータΦにより、各関節角度θ１，θ２，θ３，…，θ７を算出する。
【００４０】
【数２】
2014159066.tif 000004
また、上記式（１）、式（２）からは、式（３）及び式（４）を得ることができる。また、式（３）、式（４）において、Ｊはヤコビ行列である。
【００４１】
【数３】
2014159066.tif 000005
【００４２】
【数４】
2014159066.tif 000006
このため、ｄΘは、式（５）で表すことができる。
【００４３】
【数５】
2014159066.tif 000007
ＣＰＵ９１は、この式（５）を使用して、これよりニュートンラプソン法、或いは準ニュートン法等の収束計算を行って、手先位置姿勢ＸからベクトルΘを求める。
【００４４】
（Ｓ７０）
Ｓ７０ではＣＰＵ９１は、Ｓ６０で求めた、目標軌道全体において、最も回転角度の変動が少ない関節軸を、固定軸に設定するとともに、固定軸となる期間を検出する。ここで、固定軸なる期間の検出は、サーボロックする期間の検出に相当する。また、最も回転角度の変動が少ないとする条件は拘束条件に相当する。
【００４５】
（Ｓ８０）
Ｓ８０ではＣＰＵ９１は、固定軸に設定された関節軸以外の残りの６軸の関節軸を対象として、前記目標軌道における各点（なお、各点には教示点及び補間点を含む）における手先位置姿勢から逆変換演算を行い、関節角度を求める。すなわち、６軸の関節軸の回転角度の再算出を行う。なお、各点における６軸の関節軸の関節角度の求め方は公知であるため、説明を省略する。
【００４６】
さらに、Ｓ８０では、ＣＰＵ９１は、前記６軸の関節軸のそれぞれにおいて、前回の補間周期で求めた関節角度（前回値）と、今回の補間周期で求めた関節角度（今回値）を比較し、前回値が正で今回値が負、または前回値が負で今回値が正に変わっているかを判定する。すなわち、ＣＰＵ９１は、上記のようにして前回値が正で今回値が負、または前回値が負で今回値が正に変わっているかの判定は、関節軸の移動方向の算出結果に相当する。そして、求めた今回値の正または負が、前回値から変わっている関節軸を反転する関節軸として選択する。この選択は選択段階に相当する。
【００４７】
（Ｓ９０）
Ｓ９０では、ＣＰＵ９１は、指令生成及び指令出力を行う。すなわち、制御指令（位置指令）を生成して出力する。これにより、固定軸に設定した関節軸に対しては、サーボロックが行われるとともに、前記６軸の関節軸に対して、Ｓ８０で求めた関節角度に基づく制御指令を生成して当該関節軸のサーボモータを制御する。また、ＣＰＵ９１は、前記補間周期カウンタに１を加算して、Ｓ４０に戻る。
【００４８】
前記制御指令の生成及び出力についての詳細は後述する。
（指令生成及び出力）
次に、Ｓ９０の処理の詳細を図４のフローチャートに従って説明する。
【００４９】
（Ｓ１００）
Ｓ１００では、ＣＰＵ９１は、Ｓ８０で求めた各関節軸の制御指令（位置指令）の軸分配処理を行う。
【００５０】
（Ｓ１１０）
Ｓ１１０では、ＣＰＵ９１は、軸分配された制御指令（位置指令）を加減速処理してＳ１２０に移行する。加減速処理された位置指令は、Ｓ１０で読み込まれた移動速度（動作条件）となるように、加速、減速の少なくともいずれか一方の処理を行うものである。この加減速処理により、位置指令の結果得られる速度パターンは種々のパターン（例えば台形パターン、三角形パターン等）が得られることになる。加減速処理は、例えば、移動平均型加減速処理、２段移動平均型加減速処理等があるが、限定されるものではない。
【００５１】
前記加減速処理された位置指令は、図示しない補間バッファに格納される。
（Ｓ１２０）
Ｓ１２０では、ＣＰＵ９１は、軸分配された当該関節軸が、Ｓ８０で選択した反転する関節軸である場合には、Ｓ１４０に移行し、反転しない関節軸については、そのままＳ１８０に移行する。
【００５２】
（Ｓ１３０）
Ｓ１３０では、反転する関節軸について、位置指令の前回値（絶対値）と今回値（絶対値）の差分（＝｜前回値｜−｜今回値｜）を算出し、Ｓ１４０に移行する。
【００５３】
（Ｓ１４０）
Ｓ１４０では、ＣＰＵ９１は速度フィードフォワード設定が必要か否かを判定する。具体時には、ＣＰＵ９１は、前記差分に基づいて、減速（すなわち、減速開始及び減速中）、加速、定速の判定を行うとともに、加速の場合は、加速開始から所定期間中か否かの判定を行う。前記差分が０の場合は定速、当該差分が正の場合は減速、さらに当該差分が負の場合は、加速である。所定期間中か、否かの判定は、図示しないタイマが計時したカウント値に基づいて判定する。カウント値がある場合は、所定期間内であるとし、カウント値がリセットされている場合は、所定期間経過していると判定する。前記タイマは、補間周期よりも短い周期で発生するクロックパルスに基づいて計時する。なお、前記タイマの計時の開始及びリセットについては後述するＳ１８０で説明する。
【００５４】
ここで前記速度パターンについて説明する。例えば、位置指令の結果得られる速度パターンが台形パターンの場合、図６の二点鎖線で示すように加速期間、定速期間、減速期間を有する。また、前記速度パターンが三角形パターンの場合、図示はしないが、速度パターンは加速期間及び減速期間を有する。なお、図６の実線で示すように実速度では、反転開始のための位置指令を出力直後からの制御遅れ時間を経過した後に、サーボモータに連係されているギヤによるバックラッシュ期間がある。
【００５５】
ＣＰＵ９１は、例えば、前記速度パターンが台形パターンの場合、速度が定速期間から減速期間に移行した場合は、前記差分は正となる。このため、ＣＰＵ９１はこれを減速と判定して、速度フィードフォワード設定が必要としてＳ１５０に移行する。  また、前記差分が負の場合、加速であるため、速度フィードフォワード設定が必要でないと判定する。この加速の場合には、さらに、タイマが計時したカウント値が、加速開始のための位置指令を後述するＳ１８０のステップで出力してから所定カウント値（すなわち、所定期間）内である場合には、Ｓ１６０に移行し、所定期間経過している場合には、Ｓ１７０に移行する。
【００５６】
前記所定期間は、Ｓ１８０において反転開始のための位置指令が出力されてから、前記バックラッシュ期間の終了時までを含む期間であって、試験等により得られた期間である。バックラッシュ期間は、各関節軸で使用するサーボモータに連係するギヤの仕様が異なることもあるため、前記所定期間は各関節軸のギヤの仕様に対応するように設定されている。
【００５７】
（Ｓ１５０）
Ｓ１５０では、ＣＰＵ９１は速度フィードフォワード制御（図５では速度Ｆ／Ｆとして図示している。）を有効に設定し、Ｓ１８０に移行する。速度フィードフォワード制御の速度フィードフォワードゲインは、予め試験等により得られた値を使用する。
【００５８】
（Ｓ１６０）
Ｓ１６０では、ＣＰＵ９１は、速度フィードフォワード制御を無効とし、速度制御ループの通常に使用する積分ゲインＫ１２ａから当該積分ゲインＫ１２ａよりも大きな積分ゲインＫ１２ｂに変更するように設定し、Ｓ１８０に移行する。
【００５９】
（Ｓ１７０）
Ｓ１７０では、ＣＰＵ９１は、速度フィードフォワード制御を無効とし、速度フィードバック制御の積分ゲインＫ１２ｂを元の積分ゲインＫ１２ａに戻す、すなわち積分ゲインを小さく設定し、Ｓ１８０に移行する。
【００６０】
なお、上記のようにＳ１５０，Ｓ１６０，Ｓ１７０において設定した各項目は、制御パラメータとして図示しないバッファに格納される。
（Ｓ１８０）
Ｓ１８０では、Ｓ１１０で加減速処理が行われて前記補間バッファに格納された位置指令θｓ＊を補間周期毎に図５に示す各関節軸の位置制御部１１０に出力するとともに、前記位置指令θｓ＊がＳ１５０〜Ｓ１７０で設定された制御パラメータのいずれかと関連している場合には、前記バッファに格納した制御パラメータに基づく制御を実行する。
【００６１】
ここで、前記タイマは、今回値の位置指令を出力する時に、前回値の位置指令が減速であって、今回値が加速の場合に前記タイマの計時をスタートする。なお、前記タイマは、所定期間に達した場合にはリセットされる。
【００６２】
（Ｓ１９０）
Ｓ１９０では、指令された位置まで位置指令の分配が完了したか判断し、分配完了があるまで、Ｓ１００からＳ１４０までの処理を繰り返し実行し、分配完了があると、この処理を抜け出る。
【００６３】
（位置制御、速度制御及び電流制御）
前記制御指令、すなわち位置指令θｓ＊に基づく位置制御、速度制御及び電流制御について図５を参照して説明する。
【００６４】
図５に示すように、ＣＰＵ９１は、位置制御部１１０、速度制御部１２０及び電流制御部１３０としてそれぞれ位置制御、速度制御及び電流制御を実行する。なお、図５は、一点鎖線から左側は、コントローラＲＣ側の動作を表わし、右側は、サーボモータ側の動作を表わしている。
【００６５】
位置指令θｓ＊は、前記作業プログラムに記述されているエンドエフェクタ(ツール４９)の目標位置及び目標姿勢に基づいて逆変換演算が行われて求められた各サーボモータの回転位置である。
【００６６】
この位置指令θｓ＊と、ロータリエンコーダ７１〜７７にて求めた各サーボモータ４１〜４７の実位置θｋとの偏差を算出する。なお、前記ロータリエンコーダ７１〜７７は作業プログラムでの補間周期よりも、十分に短い検出周期で回転角度（すなわち、関節角度）を検出している。
【００６７】
そして、この算出した位置偏差に、位置制御部１１０では、Ｐ制御（比例制御）にて、予め設定されている位置ゲインＫ０を乗じて、サーボモータの目標速度（速度指令ωｓ＊）を算出する。位置ゲインＫ０は、位置制御ループのゲインである。
【００６８】
ここで、速度フィードフォワード制御が行われていない通常の場合には、さらにこの速度指令ωｓ＊と、実位置θｋから求められた各サーボモータ４１〜４７の実速度ωとの速度偏差に、速度制御部１２０では、ＰＩ制御により、予め設定されている所定の速度ゲインＫ１を乗じて、各サーボモータ４１〜４７に対する電流指令ｉｑ＊を算出し、この電流指令ｉｑ＊を電流制御部１３０に出力する。ここでの速度ゲインＫ１は、ＰＩ制御における比例制御の比例ゲインＫ１１、及び積分制御の積分ゲインＫ１２ａを総合したものであり、各ゲインは予め設定されたものである。ここでは、実速度ωがフィードバックされた速度制御ループが構成されている。
【００６９】
また、速度フィードフォワード制御が行われている場合（前記Ｓ１５０で速度フィードフォワードが有効と設定された場合）には、位置制御部１１０からの位置指令θｓ＊にゲインＫｆｆ・ｓ（微分演算子）を乗算することによりフィードフォワード量ｕを算出する。このフィードフォワード量uは、前記減速領域においてエンドエフェクタ（ツール４９）の速度変動等の制御性能が劣化しない量である。なお、ゲインＫｆｆは速度フィードフォワードゲインである。
【００７０】
従って、速度フィードフォワード制御が行われている場合には、速度制御部１２０は、速度指令ωｓ＊と実速度ωとの速度偏差にフィードフォワード量ｕが加算した値が入力される。
【００７１】
また、速度フィードフォワード制御が有効から無効になった場合には、前記速度ゲインＫ１は、ＰＩ制御における比例制御の比例ゲインＫ１１、及び積分制御の積分ゲインＫ１２ｂ（＞Ｋ１２ａ）を総合したものとなる。
【００７２】
電流制御部１３０は、図示しない電流検出回路にて検出された各サーボモータ４１〜４７の実電流をＡ／Ｄ変換して取り込み、この実電流ｉｑが電流指令となるように、制御指令を出力して図２に示す前記ＰＷＭジェネレータ５１〜５７にてＰＷＭ信号を生成する。ここでは、実電流ｉｑがフィードバックされた電流制御ループが構成されている。
【００７３】
すなわち、電流指令ｉｑ＊と実電流ｉｑとの電流偏差に対して、ＰＩ制御にて所定の電流ゲインＫ２を乗じることにより、ＰＷＭ信号を生成する。そして、この生成したＰＷＭ信号を各サーボアンプ６１〜６７に出力し、各サーボモータ４１〜４７の通電電流を制御する。
【００７４】
なお、図５において、実電流をＡ／Ｄ変換にて取り込む手順を表わすブロックに記載のＫＡ/Ｄ は、実電流をデジタル値に変換する際の変換定数を表わす。
この結果、各サーボモータ４１〜４７のモータ巻線には、各サーボアンプ６１〜６７から、ＰＷＭ信号に応じて駆動電圧が印加され、モータ巻線の端子電圧は、この駆動電圧と、モータの回転角速度に逆起電力定数Ｋｅを乗じて得られる逆起電圧とを合成した電圧となる。そして、各モータ巻線には、この端子電圧に、モータインダクタンスＬ及びモータ抵抗Ｒをパラメータとする係数｛１／（Ｌｓ＋Ｒ）｝を乗じた電流（つまり実電流）が流れる。
【００７５】
また、モータ巻線に電流が流れると、各サーボモータ４１〜４７においては、回転子に、実電流とトルク定数Ｋｔとにより決定されるモータトルクＴＭが発生し、モータ軸のイナーシャＪによる遅れ（１／Ｊ）を伴って回転角加速度が発生し、その回転角加速度を積分（１／ｓ）した回転速度に制御される。そして、この回転速度を積分（１／ｓ）した回転位置が、各サーボモータ４１〜４７に設けられたロータリエンコーダ等のセンサにて検出され、その検出信号がコントローラＲＣ内にフィードバックされる。
【００７６】
このようにして、コントローラＲＣは、各サーボモータ４１〜４７の回転位置，速度をフィードバック制御するサーボ系の制御装置として構成され、各サーボモータ４１〜４７の回転位置、及びツール４９の位置を制御する。
【００７７】
このような作業プログラムは、例えば、ＰＴＰ（ｐｏｉｎｔ  ｔｏ  ｐｏｉｎｔ）動作が要求される場合において、マニピュレータの教示点から教示点への移動が迅速に行われるため有効である。また、作業プログラムとして、ＰＴＰ（ｐｏｉｎｔ  ｔｏ  ｐｏｉｎｔ）動作以外の動作においても、７軸ロボットを６軸ロボットとして使用させたい場合にも有効である。
【００７８】
本実施形態では、下記の特徴を有する。
（１）  本実施形態のロボット制御装置（コントローラＲＣ）は、複数の教示点に基づいて目標軌道を作成するＣＰＵ９１（目標軌道作成部）を備え、ＣＰＵ９１は、前記目標軌道に基づく関節軸（Ｊ１軸〜Ｊ７軸）の移動方向の算出結果に基づいて、反転する関節軸を選択する。また、ＣＰＵ９１（制御部）は、選択した関節軸のサーボモータ４１〜４７の反転のための減速期間中は、速度制御ループへの速度指令ωｓ＊に速度フィードフォワード制御に基づく値を加算し、減速期間経過後は速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻すことを行う。
【００７９】
この結果、本実施形態によれば、関節軸のサーボモータの反転のための減速期間中は速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算するため、減速を早期に完了することができる。また、本実施形態によれば、減速期間経過後の所定期間内に含まれるギヤのバックラッシュ期間では負荷側のイナーシャがなくなるが、このバックラッシュ期間では速度制御ループの積分ゲインを大きくするため、バックラッシュ期間を通過後の加速開始を早期に行うことができる。このため本実施形態では、関節軸が反転を行う際のエンドエフェクタの速度変動の制御性能の劣化を防止することができる。
【００８０】
（２）  本実施形態のロボットの制御方法によれば、複数の教示点に基づいて目標軌道を作成する目標軌道作成段階（Ｓ３０）と、目標軌道に基づく関節軸（Ｊ１軸〜Ｊ７軸）の移動方向の算出結果に基づいて、反転する関節軸を選択する選択段階（Ｓ８０）と、選択した関節軸のサーボモータ４１〜４７の反転のための減速期間中は、前記速度制御ループへの速度指令に速度フィードフォワード制御に基づく値を加算する。そして、減速期間経過後は速度制御ループの積分ゲインを大きくし、所定期間経過後に前記積分ゲインを元の大きさに戻す段階を有する。この結果、上記（１）の効果を奏することができる。
【００８１】
（３）  本実施形態の多関節ロボットが冗長ロボットであり、ＣＰＵ９１（選択部）は、最も回転角度の変動が少ないとする条件（拘束条件）を満足かる関節軸を固定軸としてその固定軸を除いた残りの関節軸の移動方向の算出結果に基づいて、反転する関節軸を選択する。この結果、冗長ロボットにおいて、上記（１）の効果を奏することができる。
【００８２】
なお、本発明の実施形態は前記実施形態に限定されるものではなく、下記のように変更しても良い。
・  前記実施形態では、７軸マニピュレータのロボット制御装置に具体化したが、６軸以下のマニピュレータを制御するロボット制御装置、または８軸以上のマニピュレータを制御するロボット制御装置に具体化してもよい。
【００８３】
・  前記実施形態では、サーボモータをＡＣモータとしたが、ＤＣモータに変更してもよい。
・  前記実施形態では、速度制御部１２０は、ＰＩ制御を行うようにしたが、ＰＩＤ制御を行うようにしてもよい。
【符号の説明】
【００８４】
ＲＣ…コントローラ（ロボット制御装置）、
１０…マニピュレータ、４１〜４７…サーボモータ、
９１…ＣＰＵ（目標軌道作成部、選択部、及び制御部）。
【図１】
2014159066.tif 000008
【図２】
2014159066.tif 000009
【図３】
2014159066.tif 000010
【図４】
2014159066.tif 000011
【図５】
2014159066.tif 000012
【図６】
2014159066.tif 000013