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