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