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