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