JP2015202536A - 作用された力に応じて移動されるロボットを制御するロボット制御装置 - Google Patents

Abstract

【課題】入力装置を用いずにロボットの先端部の位置姿勢を移動させる。【解決手段】ロボット制御装置（１０）の操作指令部（２４）は、特異姿勢近傍判定部（２７）によりロボットが特異姿勢の近傍にないと判定されるとき、ロボットの先端部の位置および／または姿勢を、第一力算出部（２２）が算出した前記操作力に基づいて移動させる操作指令を出力する第一制御モードと、特異姿勢近傍判定部によりロボットが特異姿勢の近傍にあると判定されるとき、操作軸設定部（２５）によって設定される操作軸の位置を、第二力算出部（２３）が算出した操作力と操作軸設定部によって設定される移動方向に基づいて移動させる操作指令を出力する第二制御モードとを有する。【選択図】図２

Description

本発明は、ロボットに作用された力に応じて移動されるロボットを制御するロボット制御装置に関する。さらに特別には、本発明は、複数の軸から成るロボットに作用させた力に基づいて、前記ロボットを移動させるロボットシステムのロボット制御装置に関する。

ロボットに力を作用させることにより、ロボットを移動させるロボットの操作方法や、ロボットを移動させて位置を教示する方法として、ダイレクトティーチが知られている。このような場合には、ロボットを移動させたい方向に力を作用させてロボットを直接的に誘導し、それにより、ロボットを直交座標系上における所望の位置および／または姿勢に移動させることができる。

特許文献１には、ロボットの腕の先端に取付けられた力検出器の手動操作部を操作したときに、力検出器により発生する信号をもとに、ロボットの腕の先端部の位置および姿勢を移動させる方法が開示されている。

特許文献２に開示される直接教示装置においては、人為的に加えられた力をロボットに設けられた力センサにより検出し、その力の信号を用いてロボットの位置および姿勢を移動させる際に、デカルト座標系上の特定方向にのみ、ロボットアームを移動させている。

特許文献３には、ダイレクトティーチで移動させている際、ロボットの先端部が特異点で実現できない速度成分を、特異点に近付くにつれてあらかじめ定められた補正式により減衰させてゆくようにする、ダイレクトティーチにおける特異点回避方法が開示されている。ここで、特異点とは、ロボットが特異姿勢となる状態である。
特異姿勢とは、ロボットの先端部直交座標系上の位置および／または姿勢を各軸の位置に一意に逆変換することができない状態となる姿勢、また、ロボットの先端部直交座標系上の位置および／または姿勢をある方向に移動させることができない状態となる姿勢、また、ロボットの先端部の速度と関節速度の関係を表すヤコビ行列がフルランクでない状態となる姿勢である。このような特異姿勢の近傍の位置および／または姿勢においては、直交座標系上の位置および／または姿勢を変えることが困難であったり、過大な速度で動く軸が存在したり、位置を制御するときに不安定になりやすかったりする。このため、直接教示においては操作者が危険にさらされる場合がある。これに対して、従来技術においては、ダイレクトティーチ中に、減速停止させる方法や、特異姿勢を回避するように軌跡をずらす方法などが提案されている。

特開昭５６−８５１０６号公報 特開平６−２５０７２８号公報 特開平１１−２３９９８８号公報

特許文献１においては、力に応じて、直交座標系上のロボットの先端部の位置および／または姿勢を移動させている。このため、特許文献１においては、特異姿勢の近傍ではロボットの先端部を移動させることはできない。また、特許文献１においては、ダイレクトティーチ中に、所望の各軸の位置を移動させることができない。

特許文献２に記載の装置では、ダイレクトティーチによりロボットを移動させるときに、その移動方向を特定方向に制限して、操作性を向上させている。特定方向は、デカルト座標系上でのロボットの先端部の位置および／または姿勢に関する方向である。このため、特許文献２においては、ダイレクトティーチ中に、各軸の制御に切替えたり、或る所望の軸だけ動かすようにするなど、駆動されるべき軸を制限することなどは提案されていない。

特許文献３においては、特異点で実現できない速度成分を零にすることによって、特異点を通過しないように軌跡を変えている。このため、特許文献３においては、ダイレクトティーチにおいて、ロボットに力を作用させた方向とは異なる方向に軌跡がずれ、ロボットが操作者の意図しない位置に移動する可能性がある。また、或る作業においては、ロボットを特異姿勢の近傍に移動させることが要求される。しかしながら、そのような場合であっても、特許文献３においてはロボットを特異姿勢およびその近傍に移動させることができない。

本発明はこのような事情に鑑みてなされたものであり、ロボットの先端部に力を作用させてロボットを移動させることにより、直交座標系上におけるロボットの先端部の位置および／または姿勢を移動させる際に、ロボットの先端部を移動が困難な位置、または、移動が不可能な位置に移動させることのできるロボット制御装置を提供することを目的とする。また、ロボットの先端部に力を作用させて直交座標系上での位置および／または姿勢を移動させているときに、特別な入力装置などを用いずに、また、移動方法の切替えの入力作業などを行わずに、ロボットの移動方法を切替えることが望まれている。さらに、ダイレクトティーチ中に、特異姿勢の近傍であっても、ロボットを安定に移動させることも望まれている。

前述した目的を達成するために１番目の発明によれば、複数の軸から成るロボットに作用させた力に基づいて、前記ロボットを移動させるロボットシステムの、ロボット制御装置において、前記ロボットの先端部に作用させた力を計測する力計測部と、前記力計測部が計測した力をもとに、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方の移動操作を行う操作力を算出する第一力算出部と、前記力計測部が計測した力をもとに、前記ロボットの各軸の位置の移動操作を行う操作力を算出する第二力算出部と、前記力計測部が計測した力に基づいて前記ロボットを移動させる操作指令を出力する操作指令部と、前記ロボットが特異姿勢の近傍にあるか否かの判定を行う特異姿勢近傍判定部と、前記特異姿勢近傍判定部によって前記ロボットが特異姿勢の近傍にあると判定されるときの各軸の現在位置に応じて、前記特異姿勢の近傍にあることの要因となる軸、または、前記特異姿勢の近傍を通過させる軸、を含む一つ乃至複数の所定の軸を、力に応じて移動させる操作軸として設定し、さらに力の方向に応じて定まる前記操作軸の移動方向を設定する操作軸設定部と、を備え、前記操作指令部は、前記特異姿勢近傍判定部により前記ロボットが特異姿勢の近傍にないと判定されるとき、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方を、前記第一力算出部が算出した前記操作力に基づいて移動させる操作指令を出力する第一制御モードと、前記特異姿勢近傍判定部により前記ロボットが特異姿勢の近傍にあると判定されるとき、前記操作軸設定部によって設定される前記操作軸の位置を、前記第二力算出部が算出した前記操作力と前記操作軸設定部によって設定される前記移動方向に基づいて移動させる操作指令を出力する第二制御モードとを有する、
ことを特徴とするロボット制御装置が提供される。
２番目の発明によれば、複数の軸から成るロボットに作用させた力に基づいて、前記ロボットを移動させるロボットシステムの、ロボット制御装置において、前記ロボットの先端部に作用させた力を計測する力計測部と、前記力計測部が計測した力をもとに、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方の移動操作を行う操作力を算出する第一力算出部と、前記力計測部が計測した力をもとに、前記ロボットの各軸の位置の移動操作を行う操作力を算出する第二力算出部と、前記力計測部が計測した力に基づいて前記ロボットを移動させる操作指令を出力する操作指令部と、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあるか否かの判定を行う軸位置状態判定部と、前記軸位置状態判定部によって前記所定の軸が前記状態の位置にあると判定されるときの前記ロボットの各軸の現在位置に応じて、前記状態の位置にあることの要因となる軸、または、前記状態の位置を通過させる軸、を含む一つ乃至複数の所定の軸を、力に応じて移動させる操作軸として設定し、さらに力の方向に応じて定まる前記操作軸の移動方向を設定する操作軸設定部と、を備え、前記操作指令部は、前記軸位置状態判定部により前記所定の軸が前記状態の位置にないと判定されるとき、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方を、前記第一力算出部が算出した前記操作力に基づいて移動させる操作指令を出力する第一制御モードと、前記軸位置状態判定部により前記所定の軸が前記状態の位置にあると判定されるとき、前記操作軸設定部によって設定される前記操作軸の位置を、前記第二力算出部が算出した前記操作力と前記操作軸設定部によって設定される前記移動方向に基づいて移動させる操作指令を出力する第二制御モードとを有することを特徴とするロボット制御装置が提供される。
３番目の発明によれば、１番目または２番目の発明において、前記操作軸設定部は、前記第二制御モードになる直前または前記第二制御モードになったときの、前記操作軸にかかる力の方向と、前記操作軸の移動方向とをもとに、前記操作軸にかかる力の方向に応じて定まる移動方向を設定する。
４番目の発明によれば、１番目から３番目のいずれかの発明において、前記操作指令部は、前記第二制御モードにおいて、前記一つ乃至複数の操作軸の内、軸にかかる力の方向とは逆方向に移動させ操作軸の原点の位置を、前記操作軸の動作によって前記ロボットの先端部の位置が移動する方向とは逆方向の成分を含む方向に移動させる、または、前記操作軸にかかる力の方向の成分を含む方向に移動させる。
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記操作指令部は、前記第一制御モードと前記第二制御モードとの二つの制御モードにおいて、一方の制御モードから他方の制御モードに切替えるとき、全ての軸を減速停止または全ての軸の速度を所定閾値より小さくした後に切替える。
６番目の発明によれば、１番目から５番目のいずれかの発明において、前記操作指令部は、前記第一制御モードと前記第二制御モードとの二つの制御モードにおいて、前記第一制御モードから前記第二制御モードに切替えるとき、前記第二制御モードで動作させる軸以外の軸を減速停止させる。
７番目の発明によれば、１番目から６番目のいずれかの発明において、前記第一制御モードと前記第二制御モードの内、どちらの制御モードであるかの表示出力を行うともに、前記第二制御モードのとき、前記操作軸設定部が設定する前記操作軸と、該操作軸にかかる力の方向に応じて定まる移動方向の表示出力を行う表示出力部を備える。

１番目の発明によれば、ロボットの現在の軸の位置をもとに操作されるべき軸を選定し、所望の軸や、ロボットの特異姿勢を通過させるような軸を、ロボットの先端部に作用させた力に応じて移動させることが可能となる。
また、ロボットの軸の位置をもとに、直交座標系上の位置および／または姿勢の移動を行う第一制御モードと、各軸の位置の移動を行う第二制御モードとを切替えることができ、また各軸を操作する場合の軸を指定することもできる。このため、ロボットの先端部に力を作用させて移動させているときに、別途用意した入力装置を用いて入力作業を行うことなしに、制御モードを切替えることが可能となる。
また、ロボットの先端部に力を作用させるという操作を行いながら、直交座標上の位置および／または姿勢を移動させたり、所望の各軸を移動させたりするということが可能となる。例えば、ロボットの先端部に力を作用させて直交座標系上のロボットの位置および／または姿勢を移動させているときに、ロボットの先端部に力を作用させながら所望の各軸を移動させて、直交座標系上の通常の移動では移動させられない位置を通過させ、或る位置から通常、移動可能でない位置に到達させられる。その後、直交座標系上の移動を再度行うことなどが可能となる。
これによって、操作者が設定を切替える教示装置を別に用意したり、設定を切替えるための入力操作をしたりすることがない。従って、ロボットシステムのコストを抑え、移動操作において、直交座標系上の任意の位置や姿勢、また、任意の軸の位置に移動させやすくなるとともに、そのような移動操作をよりスムーズに連続的に且つ快適に行うことが可能となる。
また、直交座標系上の位置および／または姿勢の移動操作が不可能か困難である、または、移動操作が不安定となる特異姿勢の近傍を通過させることが可能となり、そのような状態での移動操作を容易に行うことが可能となる。さらに、直交座標系上の位置および／または姿勢の移動操作が不安定になりやすい、特異姿勢の近傍を早期に検出して、所望の各軸を移動させることによって、ロボットを安定的に、安全に移動させることも可能となる。
また、通常の直交座標系上の位置および／または姿勢の移動操作では、移動させないような位置や、特異姿勢の近傍といった状態を利用して、移動操作中に、移動させる制御モードを切替えることによって、制御モードの切替えが分かりやすくなっている。
また、特異姿勢の近傍を制御モードの切替えの位置として用いる場合、特異姿勢近傍はロボットの軸の位置を定める形態が変わる場所でもある。従って、これによってロボットを別の形態に移動させることができる。ここで述べるロボットの「形態」とは、ロボットの先端部を或る位置および姿勢にするときに、軸の位置が一意に定まらず、軸の位置をどのようにするかという軸の状態を定める形態とする。つまり、ロボットの先端部に力を作用させて、直交座標系上で移動させているときに、一旦、特異姿勢の近傍に移動させて、ロボットの先端部に力を作用させて、各軸を移動させて、直交座標系上の移動操作では移動させることができない位置を通過させて、別の形態に移動させることによって、別の形態での直交座標系上の移動操作を行うことが可能となる。
また、特異姿勢近傍やその手前では、直交座標系上の位置を少し移動させただけでも、回転軸が意図せずに大きく回転してしまうことがある。このときに、特異姿勢近傍で各軸の力制御に切替え、意図せずに移動した各軸をもとの所望の位置にすることができる。
また、先端部に力を作用させてロボットを直交座標系上で移動させているときに、意図的に、各軸を、通常の直交座標系上での動作では移動させないような位置に変えることによって、先端部に力を作用させるという同様の操作によって、先端部に作用させた力に応じて、各軸を移動させることができる。これによって、ロボットの姿勢を任意の姿勢へ移動しやすくする。
２番目の発明においては、軸位置状態判定部を用いて、ロボットの軸の位置をもとに直交座標系上のロボットの先端部の位置および／または姿勢の移動を行う第一制御モードと、ロボットの各軸の位置の移動を行う第二制御モードとを切替えることができ、また、各軸を操作する場合の軸を指定することもできる。このため、ロボットの先端部に力を作用させて移動させているときに、別途用意した入力装置を用いて入力作業を行うことなしに、制御モードを切替えることが可能となる。
また、ロボットの先端部に力を作用させるという操作を行いながら、直交座標上の位置および／または姿勢を移動させたり、所望の各軸を移動させたりすることが可能となる。例えば、ロボットの先端部に力を作用させて、直交座標系上のロボットの先端部の位置および／または姿勢を移動させているときに、ロボットの先端部に力を作用させながら所望の各軸を移動させて、直交座標系上の通常の移動では移動させられない位置を通過させ、或る位置から通常、移動可能でない位置に到達させられる。その後、直交座標系上の移動を再度行うことなどが可能となる。
これによって、操作者が設定を切替える教示装置を別に用意したり、設定を切替えるための入力操作をしたりすることがない。従って、ロボットシステムのコストを抑え、移動操作において、直交座標系上の任意の位置や姿勢、また、任意の軸の位置に移動させやすくなるとともに、そのような移動操作をよりスムーズに連続的に且つ快適に行うことが可能となる。
また、先端部に力を作用させてロボットを直交座標系上で移動させているときに、意図的に、各軸を、通常の直交座標系上での動作では移動させないような位置に変えることによって、先端部に力を作用させるという同様の操作によって、先端部に作用させた力に応じて、各軸を移動させることができる。これによって、ロボットの姿勢を任意の姿勢へ移動しやすくする。
３番目の発明においては、操作者が意図した動作を継続して行うこと、また、違和感なく操作を続けることが可能となる。
ところで、力に応じて各軸の位置を移動させるとき、操作軸を、操作軸にかかる力の方向とは逆方向に移動させた場合、操作者は操作軸が、力を作用させる方向とは逆方向に移動するので、操作上の違和感、圧迫感、困難を感じることもある。４番目の発明においては、操作者が力を作用させるときに感じる操作上の違和感、圧迫感、困難を軽減し、操作性を向上させることが可能となる。
５番目の発明においては、第一制御モードと第二制御モードとを互いに切替えるときに、安全に切替えを行うことが可能である。
６番目の発明においては、第一制御モードから第二制御モードに切替えるときに、より継続的に移動操作を行うことが可能となる。
７番目の発明においては、より簡易に移動操作を行うことが可能となる。

本発明の第一実施形態に係るロボット制御装置によって制御されるロボットを具備するロボットシステムの概略構成を示す図である。 本発明の第一実施形態に係るロボット制御装置の構成を機能的に示す図である。 第一特異姿勢を示す図である。 第二特異姿勢を示す図である。 第三特異姿勢を示す図である。 本発明の第一実施形態に係るロボット制御装置による処理の過程を示すフローチャートである。 本発明の第一実施形態に係るロボット制御装置による処理の過程を示す他のフローチャートである。 本発明の第一実施形態に係るロボット制御装置による処理の過程を示すさらに他のフローチャートである。 ロボットの先端部に力Ｆｓが作用しているのを示す図である。 図９に示される一つの操作軸に対する操作力を算出する方法を説明するための図である。 力Ｆｐの算出方法の一例を示す図である。 力Ｆｐの算出方法の別の例を示す図である。 操作軸に対する操作力を算出する方法を説明するための図である。 操作軸に対する操作力を算出する別の方法を説明するための図である。 力Ｆｐの方向を回転させる方法の一例を示す図である。 操作軸に対する操作力を算出するさらに別の方法を説明するための図である。 操作軸に対する操作力を算出する他の方法を説明するための図である。 本発明の第二実施形態に係るロボット制御装置の構成を機能的に示す図である。 本発明の第二実施形態に係るロボット制御装置による処理の過程を示すフローチャートである。 本発明の他の実施形態に係るロボット制御装置によって制御されるロボットを具備するロボットシステムにおける、表示出力部が出力する表示装置の取付位置の一例を示す図である。 本発明のさらに他の実施形態に係るロボット制御装置によって制御されるロボットを具備するロボットシステムにおける、表示出力部が出力する表示装置の取付位置の他の例を示す図である。 ロボットの部分拡大図である。 ロボットの部分拡大図である。 ロボットの部分拡大図である。 ロボットの部分拡大図である。 ロボットの他の部分拡大図である。 ロボットの他の部分拡大図である。 ロボットの他の部分拡大図である。 ロボットの他の部分拡大図である。 ロボットのさらに他の部分拡大図である。 ロボットのさらに他の部分拡大図である。 ロボットのさらに他の部分拡大図である。 ロボットのさらに他の部分拡大図である。 ロボットの部分拡大図である。 ロボットの別の部分拡大図である。 ロボットの別の部分拡大図である。 ロボットの別の部分拡大図である。 ロボットの別の部分拡大図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
以下、力とは、特に断りがない限り、力の並進方向成分および力のモーメント成分を含むものとする。また、位置および／または姿勢とは、位置または姿勢、もしくは、位置および姿勢を表すものとする。

また、以下、軸とは、ロボットを構成するリンクとリンクとをつなぐ関節部分であり、リンクとリンクとの間の位置関係や角度関係を変える部分とする。軸の位置（回転軸の場合の位置は、角度である）を変えることによって、リンクとリンクとの位置関係を変えることができ、その結果、ロボットの先端部の位置および／または姿勢を変えることができる。なお、軸とする部分とは異なる箇所に、軸の位置を移動させるためのアクチュエータが配置されていてもよい。

また、ロボットの軸の回転中心線回りに作用させる力、ロボットの軸の回転中心線回りにかかる力、または操作軸が回転軸である場合の操作軸にかかる力とは、ロボットの軸が回転軸の場合であって、座標系の１つの軸がロボットの軸の回転中心線と一致するように、ロボットの軸に対して座標系を設定するとき、その座標系上で、ロボットの軸の回転中心線と直交する平面、かつ、原点がロボットの軸の回転中心線とその平面との交点である、平面において存在する並進方向の力、または、ロボットの軸の回転中心線回りに作用する力のモーメントとする。

また、本発明において、力制御ゲインとは、作用する力に応じてロボットを移動させる力制御において、作用する力の大きさをもとに、制御周期毎の、直交座標系上のロボットの先端部の位置および／または姿勢や、ロボットの各軸の位置などの移動量を求めるような係数である。

図１は、本発明の一実施形態に係るロボット制御装置１０によって制御されるロボット５０を具備するロボットシステム１１の構成例を示す概略図である。ロボットシステム１１は、ロボット制御装置１０と、ロボット制御装置１０によって制御周期毎の各軸の位置を制御されるロボット５０と、を具備している。

ロボットシステム１１において、操作者６０がロボット５０の先端部５８に力を作用させたとき、ロボット制御装置１０は、力計測部が計測したロボット５０の先端部５８に作用された力、設定されたデータ、ロボット５０の位置データなどに基づいて、ロボット５０の各軸を移動させるアクチュエータを制御する。これにより、ロボット５０を構成する軸の位置を変え、ロボット５０を移動させられる。

ロボット制御装置１０は、演算処理装置、ＲＯＭ、ＲＡＭなどを含むハードウェア構成を有していて、後述する種々の機能を実行する。

図１に示されるロボット５０は、六つの軸によって構成される。これら六つの軸は、ロボット５０の台座５９側から順に、第一軸をＪ１軸５１、第二軸をＪ２軸５２、第三軸をＪ３軸５３、第四軸をＪ４軸５４、第五軸をＪ５軸５５、第六軸をＪ６軸５６とする。

図１の右方に示されるように、Ｊ１軸５１、Ｊ４軸５４およびＪ６軸５６は、軸と軸をつなぐ、リンク回りに回転する回転軸Ｒ１として構成される。また、Ｊ２軸５２、Ｊ３軸５３およびＪ５軸５５は、軸と軸をつなぐ、リンクとは直交する方向回りに回転する回転軸Ｒ２として構成される。

なお、図１は、ロボット５０の軸の構成を表すための簡易的な説明図である。また、軸の原点を、各軸に設定した座標系の原点、かつ、リンクとリンクが接続される点とすると、軸の原点の位置は、空間に設定した座標系上の位置として表されるものとする。なお、以下では、空間に設定した座標系を基準座標系とする。Ｊ１軸５１とＪ２軸５２の軸の原点は同じ位置にあり、Ｊ３軸５３とＪ４軸５４の原点は同じ位置にあり、Ｊ５軸５５とＪ６軸５６の原点は同じ位置にあるものとする。

本実施例では、回転軸とした軸に対して、軸の位置を移動させる、と述べる場合、軸の位置とは、回転軸の角度であり、軸の位置を移動させるとは、回転軸を回転させて位置を変えることを表すものとする。また、軸の原点の位置、と述べる場合、空間に対して設定した座標系上の、各軸に設定した座標系の原点の位置を表すものとする。また、空間に対して設定した座標系とは、空間に対して固定させた直交座標系上で、ロボット５０の先端部５８や、先端部５８をロボット５０に取付けるためのフランジ部５７、また各軸に設定した座標系などの、位置および／または姿勢を表すための座標系とする。

また、空間に対して設定した基準座標系上の、ロボット５０の位置および／または姿勢を表すため、ロボット５０に対して設定した座標系をツール座標系とする。ツール座標系の原点であり、並進移動させる点、また、回転移動させるときの中心点を制御点とする。基準座標系に平行な座標系を、制御点に設定した座標系を制御座標系とする。なお、制御点の位置は、ロボット５０に対して設定される位置であれば、任意の位置であって構わない。

本実施例では、ロボット５０の六つの軸はいずれも回転軸とするが、ロボット５０が直動軸を含んでも構わない。また、ロボット５０は、六軸構成の垂直多関節型ロボットである。しかしながら、各軸の位置が制御可能、また、直交位置が制御可能なロボットであれば、他の形態を有する任意の公知のロボットでもよい。

ロボット５０の先端部５８は、ロボット５０のフランジ部５７に取付けられている物体が存在する部分である。ロボット５０の先端部５８には、図示しないが、六軸の力センサが取付けられている。ロボット制御装置１０は、所定時間毎に検出された力センサの出力をもとに、後述する力計測部２１によって、操作者６０がロボット５０の先端部５８に作用させた力を計測する。

力計測部２１は、ロボット５０の先端部５８における力を計測する点に原点を有する座標系を設定する。そして、力計測部２１は、ロボット５０の先端部５８に作用させた力として、その座標系上の、力の並進方向の成分Ｆおよび力のモーメント成分Ｍを計測する。以下、この座標系を力計測座標系とし、座標系の原点を力計測点とする。このとき、ロボット５０の先端部５８に設定した座標系のＸ軸、Ｙ軸、Ｚ軸の力の並進方向成分をそれぞれＦｘ、Ｆｙ、Ｆｚと表すとともに、Ｘ軸、Ｙ軸、Ｚ軸回りで検出される力のモーメント成分をそれぞれＭｘ、Ｍｙ、Ｍｚと表す。

ここで、力計測点は、操作者が力を作用させる作用点、または、力センサに設定したセンサ座標系の原点、センサ座標系の軸上の点などであって構わない。

本実施例では、力の六成分を計測するが、力の並進方向成分Ｆ、または、力のモーメント成分Ｍのみを計測するようにしてもよい。また、力センサの取付位置は、ロボット５０の先端部５８に作用させた力を計測できる限り、任意の箇所であってよい。また、ロボット５０の先端部５８に作用された力を計測する計測部は、六軸の力センサの代わりに、三軸の力センサを使用してもよい。

また、力計測部２１は、ロボット５０を構成する軸を移動させるアクチュエータがモータである場合の電流値、または、軸の指令位置と実際の軸の位置との間の偏差、または、各軸に取付けられたトルクセンサの出力などをもとに、ロボット５０の先端部５８に作用される力を推定してもよい。

なお、ロボット５０の先端部５８には、ワークを加工したり、ワークを搬送したりするなどの作業を行うためのツールや、力に応じた移動操作を行うための操縦装置などが取付けられている。

ロボット５０に取付けられた力センサに対してツールや操縦装置を取付けてもよい。あるいは、ロボット５０に取付けられたツールに対して力センサを取付け、その先端側に操縦装置を取付けてもよい。

ロボット５０の先端部５８に力を作用させるとき、操縦装置を用いることなしに、力センサに取付けられたツールに対して力を作用させてもよい。あるいは、力センサに取付けられた操縦装置に対して力を作用させてもよい。

力センサに取付けられたツールや操縦装置に対して、操作者が力を作用させて、ロボット５０を移動させる場合には、力計測部２１は、力センサが検出した力に対して、力センサに取付けられたツールや操縦装置または把持したワークなどの物体が重力や慣性力（コリオリ力、ジャイロ効果を含む）などによって及ぼす影響を、必要に応じて補償する。これにより、力計測部２１は、ロボット５０の先端部５８に操作者が作用させた正味の力を計測できる。

ロボット５０の先端部５８に取付けられたツールに、力センサと操縦装置とが組合わされた装置を取付ける場合には、力センサに取付けられた物体が重力や慣性力によって力センサに及ぼす影響が小さくなる。従って、この場合には、正味の力を求める場合の誤差も小さくなる。

また、力センサと操縦装置とが組合わされた装置を、磁石やバネなどを用いてツールに取付けてもよい。この場合には、そのような装置を簡易に着脱することができる。また、力を作用させてロボット５０を移動させる場合にのみ、そのような装置を取付けることも可能となる。これによって、教示操作が不要な時には装置を取外しておいたり、そのような装置を必要に応じて他のロボットシステムで使用することが可能となる。

図２は、本発明の第一実施形態に係るロボット制御装置１０ａの構成を機能的に示す図である。図示されるように、ロボット制御装置１０ａは、後述する力計測部２１と、第一力算出部２２と、第二力算出部２３と、操作指令部２４と、操作軸設定部２５と、記憶部２６と、特異姿勢近傍判定部２７と、表示出力部７１と、を具備している。

力計測部２１は、操作者６０が、ロボット５０の先端部５８に作用させる正味の力を計測する。前述したように、力計測部２１は、ロボット５０の先端部５８に取付けられたツールや操縦装置、または把持したワークなどの物体が重力や慣性力（コリオリ力、ジャイロ効果を含む）などによって及ぼす影響を必要に応じて補償する。

力センサに取付けられた物体が及ぼす重力や慣性力の影響の補償は、次のようにして公知の方法によって行う。力センサに取付けられた物体に対して、操作者が力を作用させる前に、その物体の質量や重心を予め算出しておく。そして、算出された質量および重心と、ロボットの移動動作を参照すると共に、公知の方法、例えば特許第４２６７０２７号に開示される手法を用いて算出する。

第一力算出部２２は、力計測部２１により計測された、ロボット５０の先端部５８に作用する、力の並進方向の成分および／または力のモーメント成分を含む力をもとに、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させるための操作力を算出する。

第一力算出部２２は、例えば、次のようにして操作力を算出する。力計測部２１が計測した力を、制御座標系上の力に変換することによって、操作力を算出する。このとき、必要に応じて、操作中のロボットの移動方向、移動速度などを考慮して、操作力の方向や、大きさを調整してもよい。これにより、力に応じて移動させるときの操作性を向上させられる。

第二力算出部２３は、力計測部２１により計測された、ロボット５０の先端部５８に作用する、力の並進方向の成分および／または力のモーメント成分を含む力をもとに、ロボット５０の各軸の位置を移動させるための操作力を算出する。

第二力算出部２３は、例えば、本実施例のように移動させる軸が回転軸である場合、次のようにして操作力を算出する。
力計測部２１が計測した、ロボット５０の先端部５８に作用された実際の力をもとに操作力を算出する。または、ロボット５０の先端部５８に作用された力をもとに、移動させるべき軸に対して、仮想的に作用するとした力である仮想力を、操作力として算出する。

具体的には、次のようにして算出する。

移動させるべき軸に対して、軸の回転中心線に直交する平面に、ロボット５０の先端部５８に作用する並進方向の力を射影したとき、軸に対して、正負のどちらの回転方向を向いているかをもとに、換言すれば、軸の回転中心線回りに作用する並進方向の力の向きをもとに、軸を移動させる操作力の方向を定める。そして、力計測部２１が計測した並進方向の力の大きさ、または、前記射影した力の大きさ、または、前記射影した力の、回転中心線から射影した力の作用点までの位置ベクトルに直交する成分の大きさをもとに、操作力の大きさを求める。

また、力計測部２１が計測した、ロボット５０の先端部５８に作用される力をもとに、移動させるべき軸に対して、回転中心線回りの力のモーメントを算出して、操作力を求めてもよい。

また、力計測部２１が計測した、ロボット５０の先端部５８に作用される力をもとに、移動させるべき軸に対して、回転中心線回りの力のモーメントを算出するときに、適宜、力のベクトルや、位置ベクトルの算出方法を工夫して、操作性を向上させるようにモーメントを算出し、操作力を求めてもよい。

また、移動させるべき軸に対して、軸の回転中心線回りに作用する力のモーメントの正負をもとに、軸を移動させる操作力の方向を定めると共に、力計測部２１が計測した力の大きさをもとに、操作に応じた適切な操作力の大きさを求めてもよい。

なお、操作力の方向は、移動させるべき軸の移動方向を定めることができるような、方向、正負の符号など、順方向か逆方向を定めるものであればよい。

また、力に応じて軸を移動させるときの、ロボットの操作性を向上させるために、必要に応じて、操作中のロボットの移動方向、移動速度などを考慮して、操作力を調整することが好ましい。

本実施例では、操作される軸が回転軸である場合を示したが、操作される軸が、直動軸である場合には、軸方向の力の並進方向の成分を算出するようにする。

特異姿勢近傍判定部２７は、ロボット５０が特異姿勢の近傍にあるか否かの判定を行う。特異姿勢の近傍にあるか否かを判定する方法はいくつか存在する。まず、ロボット５０の先端部５８の速度と関節速度との関係を表すヤコビ行列をもとにするという方法がある。ロボット５０の現在の各軸の位置における、ヤコビ行列がフルランクでないときや、ヤコビ行列の行列式の値が所定閾値より小さい値であるとき、ロボット５０の姿勢は、特異姿勢近傍にあると判定できる。これによって、ロボット５０の現在の各軸の位置が特異姿勢にあるか否かを判定することができる。

前記判定方法によって、ロボット５０が特異姿勢の近傍にあると判定される場合に、後述の方法を用いて、特異姿勢近傍判定部２７がロボット５０の現在の各軸の位置をもとに特異姿勢の近傍の状態をさらに詳細に判定する。そして、後述の操作軸設定部２５が、特異姿勢の近傍にあることの要因となる軸または特異姿勢の近傍を通過させる軸、を設定するようにする。

また、ロボットの各軸の位置が、所定の位置関係の条件を満たすか否かによって、ロボットの現在の各軸の位置が特異姿勢の近傍にあるか否かを判定することもできる。この方法によって、ロボットが特異姿勢の近傍にあるか否かの判定を行うとともに、特異姿勢が、後述のどの分類に属するかを判定する。そして、後述の操作軸設定部２５が、特異姿勢の近傍にあることの要因となる軸または特異姿勢の近傍を通過させる軸、を設定するようにしてもよい。この方法について以下で説明する。

特異姿勢は、いくつかの状態に分類できる。このとき、ロボット５０の各軸の位置によっては、複数の分類に属する場合もある。特異姿勢がどの特異姿勢の状態にあるかを判定し、その結果をもとに、後述の操作軸設定部２５が操作されるべき軸を設定する。

ロボット５０が特異姿勢であるかどうか、また、特異姿勢がどの分類に属する特異姿勢であるかは、以下のようにして判定される。すなわち、特異姿勢の状態毎に、つまり、特異姿勢の分類毎に、その特異姿勢の近傍にあることを示す要因となる、一つまたは複数の軸について、軸毎の位置が、前記軸毎に設定された所定の位置から閾値以内にあるか否かという、所定の位置関係の条件を満たすか否かによって判定される。または、複数の軸の位置関係によって、基準座標系上の、或る軸の原点の位置が、所定の位置関係の条件を満たすかどうかを判定することによって、判定される。

このとき、特異姿勢の近傍にあると判定する領域を広げるように、特異姿勢近傍判定部２７で用いられる閾値を大きくしてもよい。この場合には、後述する第一制御モードによる動作が不安定になることをより安全に、早めに検出して、第二制御モードに切替えることができる。第一制御モードが不安定になる、特異姿勢の近傍にロボットが在ることを検出するため、前述のようにヤコビ行列の行列式などを用いてもよい。この場合においても、閾値を適切に設定することにより、第一制御モードによる動作が不安定になることを、早期に検出することができる。

第二制御モードにおいては、ロボットが特異姿勢の近傍であったとしても、その位置が原因で、動作が不安定になることは少ない。このため、ロボット５０が特異姿勢の近傍にあるとき、第二制御モードに切替えることによって、力に応じたロボット５０の移動操作を、より安定させ、より安全に操作することが可能となる。ただし、特異姿勢の近傍と判定する領域を広げ過ぎると、第一制御モードで移動させる領域が狭くなる。このため、特異姿勢の近傍と判定する領域を、適当な範囲になるようにしておくことが好ましい。

次に、特異姿勢の分類について説明する。さらに、後述の操作軸設定部２５が操作軸を設定するときに考慮する、特異姿勢の近傍にあることの要因となる軸または特異姿勢の近傍を通過させる軸について説明する。

本実施例では、特異姿勢は、図３に示す第一特異姿勢、図４に示す第二特異姿勢、および図５に示す第三特異姿勢の、三つに分類される。

図３に示される第一特異姿勢においては、ロボット５０のＪ１軸５１の回転中心線上に、Ｊ５軸５５の原点が存在する。ロボット５０が第一特異姿勢の近傍にあることは、空間に設定した基準座標系上で、Ｊ１軸の回転中心線上やその近傍に、Ｊ１軸からＪ５軸までを順変換したときに得られるＪ５軸の原点の位置が存在するか否かという所定の位置関係の条件を満たすかどうかによって判定できる。この場合、第一特異姿勢の近傍にあることの要因となる軸、第一特異姿勢の近傍を通過させる軸は、Ｊ２軸５２、または、Ｊ３軸５３である。

図４に示される第二特異姿勢においては、ロボット５０のＪ２軸５２の原点、Ｊ３軸５３の原点、およびＪ５軸５５の原点が直線上に存在する。ロボット５０が第二特異姿勢の近傍にあることは、Ｊ２軸５２とＪ３軸５３の角度の位置が、所定の位置関係の条件を満たすかどうかによって判定できる。具体的には、Ｊ２軸５２の原点、Ｊ３軸５３の原点およびＪ５軸５５の原点が直線上およびその近傍に存在する位置関係にあるか否かという、所定の位置関係の条件を満たすかどうかによって判定できる。この場合、第二特異姿勢の近傍にあることの要因となる軸、第二特異姿勢の近傍を通過させる軸は、Ｊ３軸５３である。

図５に示される第三特異姿勢においては、ロボット５０のＪ３軸５３の原点、Ｊ５軸５５の原点、およびフランジ部５７の原点が直線上に存在する。ロボット５０が第三特異姿勢の近傍にあることは、Ｊ５軸５５の角度の位置が、所定の位置関係の条件を満たすかどうかによって判定できる。具体的には、Ｊ３軸５３の原点、Ｊ５軸５５の原点およびフランジ部５７の原点が直線上およびその近傍に存在する位置関係にあるか否かという、所定の位置関係の条件を満たすかどうかによって判定できる。この場合、第三特異姿勢の近傍にあることの要因となる軸、第三特異姿勢の近傍を通過させる軸は、Ｊ５軸５５である。

このようにして、ロボットの現在の各軸の位置をもとに、特異姿勢近傍にあるか否か、また、どの分類に属する特異姿勢であるかを判定する。そして、特異姿勢の要因となる軸または特異姿勢の近傍を通過させる軸を決定することができる。

操作軸設定部２５は、ロボット５０が特異姿勢の近傍にあると判定されるとき、ロボット５０の現在の各軸の位置をもとに、ロボット５０が特異姿勢の近傍にあることの要因となる軸、または、特異姿勢の近傍を通過させる軸、を含む所定の軸を、力に応じて移動させる操作軸として設定する。また、力の方向に応じて定まる操作軸の移動方向を設定する。

このとき、力の方向に応じて定まる操作軸の移動方向は、予め設定された値に基づいて、または、特異姿勢の近傍になるときの操作軸の移動方向と、操作軸にかかる力の方向とに基づいて設定する。

複数の軸から軸を選択して操作軸を設定するとき、ロボット５０が特異姿勢の近傍にあるときの、現在の各軸の位置によって決まる、特異姿勢の分類をもとに、特異姿勢の要因となる軸または特異姿勢の近傍を通過させる軸を定める。そして、特異姿勢の分類に応じて、前述した軸を含む所定の軸を操作軸とする。ロボット５０の姿勢が複数種類の特異姿勢にあてはまる場合には、所定の優先順位に基づいて、優先順位の高い特異姿勢の分類をもとに操作軸の設定を行う。そして、その特異姿勢の要因となる軸またはその特異姿勢の近傍を通過させる軸を、操作軸に含めるようにする。

このとき、ロボット５０が特異姿勢の近傍にあることの要因となる軸、または、特異姿勢の近傍を通過させる軸以外の軸について、そのときのロボット５０の各軸の位置をもとに、所定の軸を操作軸として選択する。

図３に示される第一特異姿勢の場合には、特異姿勢の近傍にあることの要因となる軸、または、特異姿勢の近傍を通過させる軸は、Ｊ２軸５２またはＪ３軸５３である。Ｊ２軸５２またはＪ３軸５３のいずれか一方の軸を、力に応じて移動されるべき操作軸に設定して、移動させる。これにより、ロボット５０に対して第一特異姿勢の近傍を通過させることが可能である。

ロボット５０のＪ２軸５２およびＪ３軸５３の両方の軸の回転中心線が平行かつ同じ方向を向いているとする。このとき、Ｊ２軸５２およびＪ３軸５３の両方の軸を操作軸として移動させる場合には、これらの軸を回転中心線回りに同じ回転方向に移動させることによって、いずれか一方の軸を移動させる場合と比べて、第一特異姿勢の近傍をより素早く通過させることが可能である。また、この場合に、Ｊ２軸５２およびＪ３軸５３の両方の軸を回転中心線回りに異なる回転方向に移動させた場合であっても、それぞれを適切な速度にして移動させれば、第一特異姿勢の近傍を通過させることが可能である。前記に関連して、複数の各軸の移動操作において、複数の軸の回転中心線が平行である場合、それら複数の軸を同じ方向に回転移動させることによって、ある方向に、より素早く移動させてもよい。

特異姿勢の近傍において、各軸を移動させるとき、Ｊ２軸５２またはＪ３軸５３の回転中心線に直交する回転中心線を持つ軸を、操作軸として移動可能なようにしてもよい。この場合には、Ｊ２軸５２およびＪ３軸５３とは、独立に、その軸を移動させることが可能になる。
また、Ｊ２軸５２またはＪ３軸５３を操作軸としたとき、ロボット５０の先端部５８に作用させる力の方向によっては、Ｊ２軸５２またはＪ３軸５３とは独立して移動したり、Ｊ２軸５２またはＪ３軸５３と同時に移動したりする軸が存在する場合がある。Ｊ２軸５２またはＪ３軸５３とは独立に移動させられる軸であることが好ましいが、ロボット５０の先端部５８に作用させる力の方向によって同時に移動する場合があっても、その動作による支障がない場合は、その軸を操作軸としてもよい。このとき、その軸に対しては、操作力を小さく算出するようにしたり、操作力と力制御ゲインとをもとに移動させる場合の力制御ゲインの値を小さくしたりしておいてもよい。

ここでは、第一特異姿勢の近傍にある場合、Ｊ２軸５２およびＪ３軸５３と、Ｊ１軸５１とを、力に応じて軸の位置を移動させるべき操作軸とする。力の方向に応じて定まる操作軸の移動方向は、Ｊ２軸５２およびＪ３軸５３の両方について、力の方向と同じ方向とする。

これによって、ロボット５０の先端部５８に力を作用させ、Ｊ２軸５２およびＪ３軸５３の両方の位置を、前記のように回転中心線回りに同じ方向に移動させれば、第一特異姿勢近傍を素早く通過させることができる。

また、操作軸の回転中心線回りにかかる力を操作力として軸を移動させる場合に、Ｊ１軸５１を移動させる際に、ロボット５０の先端部５８に作用される力の方向を、Ｊ２軸５２およびＪ３軸５３の回転中心線と平行な方向や、平行に近い方向にしてもよい。この場合には、Ｊ２軸５２およびＪ３軸５３の移動量を小さくしつつ、Ｊ１軸５１を移動させることが可能となる。

また、操作軸の回転中心線回りにかかる力を操作力として軸を移動させる場合には、Ｊ２軸５２およびＪ３軸５３を移動させる際に、操作力の作用する方向を、Ｊ１軸５１の回転中心線と平行な方向や、平行に近い方向にしてもよい。この場合には、Ｊ１軸５１の移動量を小さくすることが可能となる。また、所望の軸以外の軸が移動しないように、ロボット５０の先端部５８に作用させる力の方向をもとに、所定方向との比較などによって、操作力の方向を求めるようにしてもよい。

また、複数の軸のうち、操作軸とする軸を選定するときに、ロボットが第一特異姿勢の近傍にある場合には、Ｊ２軸５２またはＪ３軸５３と、所望の軸とを、操作軸とするようにしてもよい。

Ｊ２軸５２およびＪ３軸５３の両方の軸を操作軸として移動させる場合には、両方の軸を移動させるので、操作軸設定部２５の設定、または、ロボット５０の先端部５８に作用させる力の方向、または、ロボット５０の先端部５８の位置によっては、特異姿勢の近傍を通過させるのに時間がかかる場合がある。このような場合、経過時間や、軸の移動量をもとに、Ｊ２軸５２およびＪ３軸５３のうちの一方の軸を操作軸とせず、この軸を移動させないようにしたり、力の方向に応じて定まる操作軸の移動方向を変更してもよい。

なお、Ｊ２軸５２およびＪ３軸５３の両方の軸を操作軸として移動させるとき、力の作用方向によっては、これら操作軸が互いに異なる方向に移動する場合がある。このため、操作軸を移動させる操作力を求めるときに、操作力の作用する方向を、両方の軸が同じ方向に移動するように求めるのが好ましい。

図４に示される第二特異姿勢の場合、特異姿勢の近傍にあることの要因となる軸、または、特異姿勢の近傍を通過させる軸は、Ｊ３軸５３である。Ｊ３軸５３を、力に応じて移動されるべき操作軸に設定して、移動させる。これにより、ロボット５０に対して第二特異姿勢の近傍を通過させることが可能である。

また、Ｊ２軸５２およびＪ３軸５３の両方の軸を操作軸として移動させる場合には、両方の軸の回転中心線が平行かつ同じ方向を向いている場合、これらの軸を回転中心線回りに異なる回転方向に移動させる。これによって、Ｊ３軸５３の移動によって、第二特異姿勢の近傍を通過させることができると共に、Ｊ２軸５２を逆方向に移動させることによって、基準座標系上の、Ｊ５軸５５の原点の位置、ロボット５０の先端部５８の位置の移動量を小さくすることが可能となる。これによって、ロボットの操作性を向上させることができる。

特異姿勢の近傍において、各軸を移動させるとき、Ｊ２軸５２またはＪ３軸５３の回転中心線に直交する回転中心線を持つ軸を、操作軸として移動可能なようにしてもよい。この場合には、Ｊ２軸５２およびＪ３軸５３とは、独立に、その軸を移動させることが可能になる。

また、Ｊ２軸５２またはＪ３軸５３を操作軸としたとき、ロボット５０の先端部５８に作用させる力の方向によっては、Ｊ２軸５２またはＪ３軸５３とは独立して移動したり、Ｊ２軸５２またはＪ３軸５３と同時に移動したりする軸が存在する場合がある。Ｊ２軸５２またはＪ３軸５３とは独立に移動させられる軸であることが好ましいが、ロボット５０の先端部５８に作用させる力の方向によって同時に移動する場合があっても、その動作による支障がない場合は、その軸を操作軸としてもよい。このとき、その軸に対しては、操作力を小さく算出するようにしたり、操作力と力制御ゲインをもとに移動させる場合の力制御ゲインの値とを小さくしておいてもよい。

ここでは、第二特異姿勢の近傍にある場合、Ｊ２軸５２およびＪ３軸５３と、Ｊ１軸５１とを、力に応じて位置を移動させるべき操作軸とする。力の方向に応じて定まる操作軸の移動方向は、Ｊ２軸５２は力の方向とは逆方向、Ｊ３軸５３は力の方向と同じ方向とする。また、Ｊ２軸の力の方向に応じて定まる移動方向の設定は、Ｊ３軸５３を移動させているとき、Ｊ３軸５３の移動方向とは逆方向に移動するように、適宜、設定を変更してもよい。

また、操作軸の回転中心線回りにかかる力を操作力として軸を移動させる場合に、Ｊ１軸５１を移動させる際に、ロボット５０の先端部５８に作用される力の方向を、Ｊ２軸５２およびＪ３軸５３の回転中心線と平行な方向や、平行に近い方向にしてもよい。この場合には、Ｊ２軸５２およびＪ３軸５３の移動量を小さくしつつ、Ｊ１軸５１を移動させることが可能となる。

また、操作軸の回転中心線回りにかかる力を操作力として軸を移動させる場合には、Ｊ２軸５２およびＪ３軸５３を移動させる際に、操作力の作用する方向を、Ｊ１軸５１の回転中心線と平行な方向や、平行に近い方向にしてもよい。この場合には、Ｊ１軸５１の移動量を小さくすることが可能となる。また、所望の軸以外の軸が移動しないように、ロボット５０の先端部５８に作用させる力の方向をもとに、所定方向との比較などによって、操作力の方向を求めるようにしてもよい。また、複数の軸のうち、操作軸とする軸を選定する際に、第一特異姿勢の近傍にある場合には、Ｊ３軸５３と、所望の軸とを、操作軸としてもよい。

図５示される第三特異姿勢の場合、特異姿勢の近傍にあることの要因となる軸、または、特異姿勢の近傍を通過させる軸は、Ｊ５軸５５である。Ｊ５軸５５を、力に応じて移動されるべき操作軸に設定して、移動させる。これにより、ロボット５０に対して第三特異姿勢の近傍を通過させることが可能である。

Ｊ５軸５５を操作軸としたとき、ロボット５０の先端部５８に作用させる力の方向によっては、Ｊ５軸５５とは独立して移動したり、Ｊ５軸５５と同時に移動したりする軸が存在する場合がある。Ｊ５軸５５とは独立に移動させられる軸であることが好ましいが、ロボット５０の先端部５８に作用させる力の方向によって同時に移動する場合があっても、その動作による支障がない場合は、その軸を操作軸としてもよい。このとき、その軸に対しては、操作力を小さく算出するようにしたり、操作力と力制御ゲインをもとに移動させる場合の力制御ゲインとの値を小さくしておいてもよい。

ここでは、第三特異姿勢の近傍にある場合、Ｊ４軸５４およびＪ５軸５５を、力に応じて位置を移動させるべき操作軸とする。力の方向に応じて定まる操作軸の移動方向は、Ｊ４軸５４については、力の方向と同じ方向とする。

Ｊ５軸５５の力の方向に応じて定まる操作軸の移動方向は、特異姿勢の近傍になる前、または、特異姿勢の近傍になったときの、Ｊ５軸５５の移動方向と、Ｊ５軸にかかる力の方向とをもとに設定される。もしくは、特異姿勢の近傍になる前に直交座標系上の移動操作を行っていたときは、その移動操作が並進方向の操作である場合には、操作軸の移動方向は力の方向とは逆方向にする。その移動操作が回転方向の操作である場合には、操作軸の移動方向は力の方向と同じ方向とする。これによって、特異姿勢の近傍になる前の、直交座標系上の移動操作と、力の方向に応じて定まる操作軸の移動の方向とを同じとすることができ、操作性をよくすることができる。

また、複数の軸のうち、操作軸とする軸を選定するときに、ロボット５０が第三特異姿勢の近傍にある場合には、Ｊ５軸５５と、所望の軸とを、操作軸としてよい。

図２に示される操作指令部２４は、ロボット５０の先端部５８に作用させた力に基づいてロボット５０を移動させるように、力計測部２１が計測した力をもとに、第一力算出部２２または第二力算出部２３が算出した操作力を用いて、ロボット５０を移動させる操作指令を出力する。

操作指令部２４は、ロボット５０を移動させるための、第一制御モードと第二制御モードを有する。特異姿勢近傍判定部２７によって、ロボット５０が特異姿勢の近傍にないと判定される場合には、操作指令部２４は、第一制御モードに設定し、第一制御モードに基づいてロボット５０を移動させる。そして、特異姿勢近傍判定部２７によって、ロボット５０が特異姿勢の近傍にあると判定される場合には、第二制御モードに設定し、第二制御モードに基づいてロボット５０を移動させる。

第一制御モードにおいては、操作指令部２４は、第一力算出部２２が算出した操作力をもとに、ツール座標系を、並進移動、また、制御点を回転中心点として回転移動させるようにして、直交座標系上でのロボット５０の先端部５８の移動方向、移動速度を求める。そして、操作指令部２４は、直交座標系上の、ロボット５０の先端部５８の位置および／または姿勢を移動させる操作指令を制御周期毎に出力する。このとき、直交座標系上のロボット５０の先端部５８の位置および／または姿勢の指令は、各軸の位置の指令に変換されて、出力される。また、このとき、力制御ゲインによって、操作力に対する移動速度を定めるようにしてもよい。また、移動操作時の状況に応じて、操作力に対する応答性を下げたり、速度を加速したり、減速したりと、必要に応じて、移動速度を調整することが好ましい。

第二制御モードにおいては、操作指令部２４は、操作軸設定部２５が設定する操作軸、力の方向に応じて定まる操作軸の移動方向、および、操作軸における第二力算出部２３が算出した操作力をもとに、操作軸の移動方向、移動速度を求める。そして、操作軸を移動させる操作指令を制御周期毎に出力する。操作力に基づいて操作指令を生成するときには、力制御ゲインによって、操作力に対する移動速度を定めてもよい。また、操作時の状況に応じて、操作力に対する応答性を下げたり、速度を加速したり、減速したりと、必要に応じて、移動速度を調整することが好ましい。

記憶部２６には、力計測部２１が力を計測するために必要なパラメータ、第一力算出部２２や第二力算出部２３が操作力を算出するために必要なパラメータ、操作軸設定部２５が操作軸を設定するために必要なパラメータや設定の結果、特異姿勢近傍判定部２７または軸位置状態判定部２８が判定を行うために必要なパラメータや判定結果など、各種の計算に必要なパラメータおよび計算結果などが記憶される。

表示出力部７１は、制御モードが第一制御モードと第二制御モードのどちらの制御モードであるかを出力表示する。また、制御モードが第二制御モードであるときには、表示出力部７１は操作軸設定部２５によって設定された操作軸、操作軸の力の方向に応じて定まる移動方向などを表示出力する。

さらに、ロボット制御装置１０ａは、図示しないが、ロボット５０の各軸に取付られたエンコーダなどの位置検出装置からの情報をもとに、ロボット５０の各軸の位置や、ロボット５０の先端部５８の位置および／または姿勢、速度、加速度を算出する算出部などを具備している。

さらに、ロボット制御装置１０ａは、図示しないが、各種設定を入力することのできる入力装置がロボット制御装置１０ａに接続されて転送されるデータや、他の制御装置やコンピュータで入力された設定がネットワークを通してロボット制御装置１０ａに転送されるデータなどの、入力データを受けつけて処理する入力部を具備している。

次いで、操作者６０がロボット５０の先端部５８に力を作用させてロボット５０を移動させるときの、本発明の第一の実施形態に係るロボット制御装置１０ａによる処理の過程の一例を図６、図７、図８を参照して説明する。図６、図７、図８は、ロボット制御装置１０ａによる処理の過程の一例を示すフローチャートである。以下、ロボット５０の先端部５８に力を作用させてロボット５０を移動させるときの、ロボット制御装置１０ａの実行処理を、図６、図７、図８のフローチャート及び関連する図面を参照しつつ説明する。
なお、ここで説明される一連の処理は、一例であって、本発明がこの具体例に限定されるわけではないことに留意されたい。

図６は、制御モードを設定する処理の一例を示す図である。ロボット５０を移動させる処理が開始されると、特異姿勢近傍判定部２７により、ロボット５０が特異姿勢の近傍にあるか否かの判定を行う（ステップＳ１）。この特異姿勢近傍判定部２７によって、制御周期毎の直交座標系上のロボット５０の先端部５８の位置および／または姿勢の制御を行う（第一制御モード）か、または、制御周期毎の所望の各軸の位置の制御を行う（第二制御モード）かを判定する。

特異姿勢近傍判定部２７によって、ロボット５０が特異姿勢の近傍にないと判定されるとき、制御モードを第一制御モードとする（ステップＳ２）。また、特異姿勢近傍判定部２７によって、ロボット５０が特異姿勢の近傍にあると判定されるとき、制御モードを第二制御モードとする（ステップＳ３）。

このように、特異姿勢近傍判定部２７を用いて、ロボット５０の軸の位置をもとに直交座標系上の位置および／または姿勢の移動を行う第一制御モードと、各軸の位置の移動を行う第二制御モードとを切替えることができ、また、各軸を操作する場合の軸を指定することもできる。このため、ロボット５０の先端部５８に力を作用させて移動させているときに、別に用意した入力装置を用いて入力作業を行うことなしに、制御モードを切替えることが可能となる。

また、ロボット５０の先端部５８に力を作用させるという操作を行いながら、直交座標上の位置および／または姿勢を移動させたり、所望の各軸を移動させたりすることが可能となる。例えば、ロボット５０の先端部５８に力を作用させて、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させているときに、ロボット５０の先端部５８に力を作用させながら所望の各軸を移動させて、直交座標系上の通常の移動では移動させられない位置を通過させ、或る位置から通常、移動可能でない位置に到達させられる。その後、直交座標系上の移動を再度行うことなどが可能となる。
これによって、操作者が設定を切替える教示装置を別に用意したり、設定を切替えるための入力操作をしたりすることがない。従って、ロボットシステムのコストを抑え、移動操作において、直交座標系上の任意の位置や姿勢、また、任意の軸の位置に移動させやすくなるとともに、よりスムーズに連続的に且つ快適に行うことが可能となる。

また、本発明において直交座標系上の位置および／または姿勢の移動操作が不可能か困難である、または、移動操作が不安定となる特異姿勢の近傍を通過させることが可能となり、そのような状態での移動操作を容易に行うことが可能となる。さらに、直交座標系上の位置および／または姿勢の移動操作が不安定になりやすい、特異姿勢の近傍を早期に検出して、所望の各軸を移動させることによって、ロボット５０を安定的に、安全に移動させることも可能となる。

次に、制御モードに応じて、ロボット５０を移動させる操作指令を変更する。図７は、制御モードが第一制御モードであるときの処理の一例を示す図である。以下、制御モードが、第一制御モードのときの処理について説明する。

はじめに、力計測部２１によって、ロボット５０の先端部５８に作用させた力を計測する（ステップＳ１１）。次いで、力計測部２１が計測した、ロボット５０の先端部５８に作用された力をもとに、第一力算出部２２によって、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させるための操作力を算出する（ステップＳ１２）。

次いで、操作指令部２４は、第一力算出部２２が算出した操作力をもとに、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させる操作指令を生成して、出力する（ステップＳ１３）。

図８は、制御モードが第二制御モードであるときの処理の一例を示す図である。以下、制御モードが、第二制御モードのときの処理について説明する。はじめに、力計測部２１によって、操作者６０がロボット５０の先端部５８に作用させた力を計測する（ステップＳ２１）。次いで、操作軸設定部２５は、力に応じて移動される操作軸を設定し、また、力の方向に応じて定まる操作軸の移動方向を設定する（ステップＳ２２）。

次いで、力計測部２１が計測した、ロボット５０の先端部５８に作用された力をもとに、第二力算出部２３によって、操作軸設定部２５が設定した操作軸の位置を移動させるための操作力を算出する（ステップＳ２３）。次いで、操作指令部２４は、第二力算出部２３が算出した操作力、操作軸設定部２５の設定をもとに、操作軸の位置を移動させる操作指令を生成して、出力する（ステップＳ２４）。

以下、制御モードが第二制御モードの場合における、各軸の位置を移動させる処理の実施例についてさらに説明する。ステップＳ２２において、操作軸設定部２５が、各軸の現在位置をもとに、Ｊ２軸５２、Ｊ３軸５３、Ｊ１軸５１を、操作軸に設定する。そして、操作軸設定部２５が、力の方向に応じて定まる操作軸の移動方向を、Ｊ２軸５２およびＪ３軸５３ともに、力の方向と同じ方向に設定したとする。

このような場合において、ステップＳ２３における、第二力算出部２３によって、操作軸の位置を移動させるための操作力を算出する方法、また、ステップＳ２４において操作指令部２４によって移動の操作指令を生成する方法について、詳細に説明する。

ロボット５０が、図９に示される第一特異姿勢の近傍にあるとき、第二力算出部２３が操作軸に対する操作力を算出する方法について説明する。なお、操作軸において、軸が異なる場合、また、移動操作の状況に応じて、算出方法は異なる方法を用いてもよい。

図９は、ロボット５０の先端部５８において、力Ｆｓが作用していることが、力計測部２１によって計測されていることを示す図である。力Ｆｓは、力の並進方向の成分Ｆおよび力のモーメント成分Ｍを含み、力の並進方向の成分Ｆｘ、Ｆｙ、Ｆｚおよび力のモーメント成分Ｍｘ、Ｍｙ、Ｍｚから成る力とする。

操作軸に対して座標系を設定するとき、操作軸の回転中心線と座標系のＺ軸が一致するように座標系を設定する。力計測部２１が計測した力Ｆｓを、この操作軸に設定した座標系上の力に座標変換して、算出される力における、Ｚ軸回りの力のモーメントを、操作力としてもよい。

また、操作力を次のように求めてもよい。

図１０は、図９に示した、或る操作軸３１と、力計測部２１が計測した力Ｆｓをもとに、操作軸３１に対する操作力を算出する方法を説明する図である。

操作軸３１の基準座標系上の位置を表す点Ｐ１が、座標系の原点となるように、また、Ｚ軸Ａｚが操作軸３１の回転中心線と一致するように、また、Ｘ軸ＡｘおよびＹ軸Ａｙの成す平面が、操作軸３１の回転中心線と直交する平面となるように、操作軸３１に対して、点Ｐ１、Ｘ軸Ａｘ，Ｙ軸Ａｙ、Ｚ軸Ａｚ軸で構成される座標系を設定する。平面Ｃは、操作軸３１に設定した座標系においてＸ軸ＡｘおよびＹ軸Ａｙが成す平面であり、Ｘ−Ｙ平面とする。また、平面Ｃは操作軸３１の回転中心線と直交する平面としてもよい。この場合、点Ｐ１はこの平面と回転中心線との交点になる。また、点Ｐ２は、ロボット５０の先端部５８に作用させた力を計測するときの力計測座標系の原点である、力計測点を平面Ｃに射影した点とする。

ここで、力計測部２１が計測した力Ｆｓのモーメント成分の力Ｍ（Ｍｘ、Ｍｙ、Ｍｚ）を、平面Ｃ上の力のモーメントに座標変換した力のモーメントを力のモーメントＭ２１とする。または、操作軸３１に設定した座標系上の力のモーメントに変換したときのＺ軸Ａｚ回りの力のモーメントを力のモーメントＭ２１としてもよい。

位置ベクトルＰｖは、平面Ｃにおいて、点Ｐ１から点Ｐ２への位置ベクトルとする。このときの位置ベクトルＰｖの大きさは、操作軸３１の回転中心線と力計測点との間の最短距離となる。

力Ｆｐは、力計測部２１が計測した力Ｆｓの並進方向の成分の力Ｆ（Ｆｘ、Ｆｙ、Ｆｚ）をもとに、平面Ｃ上で、操作軸３１の回転中心線回りに作用する力として求めた、並進方向の力である。並進方向の成分の力Ｆを平面Ｃに射影して得られる、並進方向の力を、力Ｆｐとしてもよい。また、力Ｆｓの並進方向の成分の力Ｆをもとに力Ｆｐを求めるとき、力Ｆｓの並進方向の成分の力Ｆが作用する方向と、所定方向への方向の回転操作とをもとに力Ｆｐを算出するようにしてもよい。

ここで、力Ｆｓの並進方向の成分の力Ｆの方向の変動によって、力Ｆによって求まる力Ｆｐの大きさが変動することを軽減させるための、力Ｆｐの算出方法の一例について図１１を用いて説明する。操作軸３１に設定した座標系上において、力計測点Ｐ３において作用する力Ｆを、力計測点Ｐ３のＺ軸Ａｚ方向の値が０となるようにした平面Ｃ上の点Ｐ２において作用するように、平行移動させた力を力Ｆｓｐとする。力Ｆｓｐと平面Ｃとの成す角度が所定閾値より小さいとき、力Ｆｓｐを、点Ｐ２を回転中心点として、平面Ｃに最も近回りする方向に回転させて平面Ｃ上の力とすることによって、力Ｆｐを求める。

なお、力Ｆｓｐと平面Ｃとの成す角度が大きく、力Ｆｓｐの方向が平面Ｃに直交する方向に近い場合、前記の方法では力を不適切に大きくしてしまうため、このようなことは行わない。力Ｆｓｐと平面Ｃとの成す角度が所定閾値以上であるとき、力Ｆｓの並進方向の成分の力Ｆまたは力Ｆｓｐを平面Ｃに射影して力Ｆｐを求めるか、または、力Ｆｐの大きさを０として力Ｆｓの並進方向の成分の力Ｆによる操作力は作用しないものとする。

また、力Ｆｓの並進方向の成分の力Ｆと平面Ｃに平行な面との成す角度が所定閾値より小さいとき、力Ｆを平面Ｃに射影し、さらに、その射影したベクトルに対して、ベクトルの大きさが力Ｆの大きさとなるように、ベクトルを伸縮調整して求めたベクトルを、力Ｆｐとしてもよい。

さらに、力Ｆｓの並進方向の成分の力Ｆと平面Ｃに平行な面との成す角度が所定閾値以上であるとき、力Ｆを平面Ｃに射影して力Ｆｐを求めるか、または、力Ｆｐの大きさを０として、力Ｆｓの並進方向の成分の力Ｆによる操作力は作用しないとしてもよい。

以上のように、力Ｆｓの並進方向の成分の力Ｆが作用する方向と、所定方向への方向の回転操作をもとに、平面Ｃ上の力Ｆｐを求めることによって、力Ｆを平面Ｃ上に射影するだけの場合に比べて、力Ｆの方向の変動による力Ｆｐの大きさの変動を軽減させたり、力Ｆｐの大きさを調整させたりすることが可能となる。

また、力Ｆｓの並進方向の成分の力Ｆの方向の変動によって、力Ｆによって求まる力Ｆｐの大きさが変動することを軽減させるための、力Ｆｐの別の算出方法の一例について図１２を参照しつつ説明する。

図１２に示す平面は、力Ｆｓの並進方向の成分の力Ｆを平面Ｃに射影したときのベクトルを含む、かつ、平面Ｃに直交する、かつ、軸Ａｚ２を含む平面を示している。ここで、軸Ａｚ２は、Ｚ軸Ａｚに平行な軸とする。操作軸３１に設定した座標系上において、力計測点Ｐ３において作用する力Ｆを、力計測点Ｐ３のＺ軸Ａｚ方向の値が０となるようにした平面Ｃ上の点Ｐ２において作用するように、平行移動させた力を力Ｆｓｐとする。この力Ｆｓｐが平面Ｃと成す角度と、所定の角度によって順次定まる所定の範囲Ｒｐ１、Ｒｐ２、Ｒｐ３とを比較して、力Ｆｓｐの方向が、所定の範囲のどの範囲に存在するかを求める。力Ｆｓｐの方向が存在する範囲をもとに、所定の範囲毎に設定した所定の角度を求めて、平面Ｃに対してこの角度を成す方向を求める。換言すれば、力の方向に応じて定まる代表的な方向を求める。そして、力の方向がこの求められた方向となるように、力Ｆｓｐを、点Ｐ２を回転中心点として回転させる。このように求められた力を平面Ｃに射影して、算出されるベクトルを、力Ｆｐとする。

例えば、力Ｆｓｐの方向が、範囲Ｒｐ１内にある場合には、力Ｆｓｐを回転して移動させる方向を平面Ｃと成す角度が０度となる方向とする。力Ｆｓｐの方向が、範囲Ｒｐ３内にある場合には、力Ｆｓｐの方向は平面Ｃと直交する方向に近い。このような場合には、力Ｆｓｐを回転して移動させる方向を平面Ｃと直交する方向とする、または、力Ｆｐの大きさを０として、力Ｆｓの並進方向の成分の力Ｆによる操作力は作用しないとする。

このように所定の範囲によって、力Ｆｓｐの方向を、切替えるときには、操作力が大きく変わらないように、力Ｆｓｐの方向、または、力Ｆｓｐを射影して求められる力Ｆｐの大きさを円滑に変化させることが好ましい。

このように求めた平面Ｃ上の力Ｆｐに対して、さらに、後述するような、所定方向への方向の回転操作や、平行移動操作、力のＦｐの大きさを変更する操作などをもとに求められた力Ｆｎを、新たな力Ｆｐとしてもよい。

そして、平面Ｃ上の力Ｆｐと、位置ベクトルＰｖとの外積演算によって求められた力のモーメントＭ１１、また、力のモーメントＭ２１をもとに操作力を求める。

位置ベクトルＰｖの大きさが所定の値より小さいとき、前記力のモーメントＭ２１を、操作力とする。これには次の効果がある。点Ｐ２が点Ｐ１の付近にあり、点Ｐ２の誤差によって、点Ｐ２が実際とは異なる位置になる、または、位置が変動してしまう場合、力のモーメントの符号が逆になることがある。このような場合であって、操作力に対する力制御ゲインの値が大きい場合、または、力のモーメントＭ１１と力のモーメントＭ２１のそれぞれに係数をかけてそれらを足し合わせて力のモーメントを算出する場合、かつ、力のモーメントＭ１１にかける係数が大きい場合に、操作軸を意図しない方向に移動させてしまうことなどを防ぐことができる。

位置ベクトルＰｖの大きさが所定の値以上であるとき、位置ベクトルＰｖと力Ｆｐの外積演算から力のモーメントを算出し、算出した力のモーメントＭ１１を操作力とする。または、前記算出した力のモーメントＭ１１と、前記力のモーメントＭ２１とを足し合わせた力のモーメントを操作力とする。さらに、力のモーメントＭ１１と、力のモーメントＭ２１のそれぞれに係数をかけることにより大きさを変えて、それぞれの影響力を調整した値を、足し合わせるようにしてもよい。このとき、この係数を、位置ベクトルＰｖの大きさや、力Ｆｐの大きさなどをもとに、調整してもよい。

力計測部２１が計測した力Ｆｓの力のモーメント成分の力Ｍの影響を排除するようにしたい場合や、力Ｆｓの並進方向の力のみで移動させたい場合には、力のモーメントＭ２１を考慮せず、力のモーメントＭ１１のみを操作力とすることが好ましい。

また、力Ｆｐの大きさが同じであっても、位置ベクトルＰｖの大きさによって、算出される力のモーメントＭ１１の大きさは変化する。このため、力のモーメントＭ２１とは異なり、力のモーメントＭ１１は、ロボット５０の先端部５８の位置の移動によって大きさが変わり、操作力が変動する。

このため、位置ベクトルＰｖの大きさが所定閾値以上で、力Ｆｐの大きさが所定閾値より小さい場合には、力のモーメントＭ２１のみを操作力とすることが好ましい場合がある。以上の、力のモーメントＭ２１についての説明は、力のモーメントＭ２１を考慮する場合、他でも同様である。

前述のように力のモーメントを算出する場合、力Ｆｐの大きさが同じであっても、位置ベクトルＰｖの大きさによって、算出される力のモーメントＭ１１の大きさが変わってしまうことに起因する問題がある。この問題に対処するための方法を、図１３を参照して説明する。

図１３は、前述のように説明した、力計測部２１が計測した力Ｆｓをもとに、第二力算出部２３が、操作軸３１に対する操作力を算出する別の方法を説明する図である。直線Ｌｗは、平面Ｃ上に存在し、点Ｐ１および点Ｐ２を含む直線とする。平面Ｃ上の並進方向の力からなる力Ｆｐが作用する、位置ベクトルＰｖをもとに、位置ベクトルＰｖの大きさを、所定の値Ｃｐｎにまで伸縮調整させた位置ベクトルＰｎを算出する。力Ｆｐを、この位置ベクトルＰｎの端点となる位置Ｐ４において作用するように、平行移動させた力を力Ｆｎとする。

このように求めた、位置ベクトルＰｎと力Ｆｎの外積演算から、力のモーメントを算出し、算出した力のモーメントＭ１２を操作力とする。

このように、力のモーメントを求めるときに、位置ベクトルを伸縮調整させることによって、点Ｐ２の位置の変動による、力のモーメントの変動を軽減させ、操作力の細かな変動や、急激な変化などを防ぐことができるため、ロボット５０を安定して移動させ、操作性を向上させることが可能となる。

また、操作力と力制御ゲインをもとに、操作軸の移動速度を求める場合、同じ大きさの力を作用させても、ロボット５０の先端部５８の位置によって、意図せずに、操作軸の移動速度が変わることを防ぐことができる。

また、図１３において、位置ベクトルＰｖの大きさを所定の大きさにするための所定の値Ｃｐｎは、位置ベクトルＰｖの大きさをもとに、変更するようにしてもよい。具体的には、位置ベクトルＰｖの大きさに対して、所定の範囲毎に段階的に、所定の値Ｃｐｎを変える。このとき、段階的に切替えるときには、操作力が大きく変わらないように、所定の値Ｃｐｎの値を円滑に変化させる、または、算出する力のモーメントＭ１２を円滑に変化させることが好ましい。

前述したように、所定の値Ｃｐｎを所定の範囲毎に段階的に変えているので、所定の範囲内での細かな位置の変動によって、力のモーメントが変動することを防ぐことができる。これによって、力Ｆｎの大きさが同じ場合であっても、位置ベクトルＰｖの大きさに応じて力のモーメントＭ１２が変わるようにできる。さらに、操作力の変動を減らし、ロボット５０をより安定、安全に移動させ、操作性を向上させることが可能となる。

あるいは、位置ベクトルＰｖの大きさが大きくなるに従って、所定の値Ｃｐｎを小さくするようにしてもよい。このとき、位置ベクトルＰｖの大きさが大きくなるに従って、位置ベクトルＰｖの大きさに対して、所定の範囲毎に段階的に、または、所定の関数に従って段階的に、所定の値Ｃｐｎが小さくなるようにする。このとき、所定の値Ｃｐｎを段階的に切替えるときには、操作力が大きく変わらないように、所定の値Ｃｐｎの値を円滑に変化させることが好ましい。

このように、所定の値Ｃｐｎを、位置ベクトルＰｖの大きさが大きくなるに従って、小さくすることによって、力Ｆｎが同じ大きさであっても、力のモーメントＭ１２の大きさを小さくして、操作力を小さくすることができる。これによって、操作力と力制御ゲインをもとに、操作軸の移動速度を求める場合、力Ｆｐが同じ大きさでも、ロボット５０の先端部５８が操作軸から遠位にあるときには、操作軸の移動速度を小さくできる。この移動速度、つまり、軸の回転速度を小さくした状態で、ロボット５０の先端部５８を移動させることによって、操作者がより安全にロボット５０を移動させることが可能となる。

さらに、図１０を参照した前述の説明と同様に、力のモーメントＭ２１を算出して用いてもよい。つまり、位置ベクトルＰｖの大きさが所定の値より小さいとき、力のモーメントＭ２１を操作力とする。また、位置ベクトルＰｖの大きさが所定の値以上であるとき、力のモーメントＭ１２、または、力のモーメントＭ１２と力のモーメントＭ２１とを足し合わせた力のモーメントを操作力とする。さらに、力のモーメントＭ１２と、力のモーメントＭ２１のそれぞれに係数をかけて大きさを変えることにより、それぞれの影響力を調整した値を、足し合わせるようにしてもよい。このとき、この係数を、位置ベクトルＰｖの大きさや、力Ｆｐの大きさなどをもとに、調整してもよい。

また、前述のように力のモーメントを算出する場合、力Ｆｐの大きさが同じであっても、力Ｆｐの方向によって、算出される力のモーメントの大きさが変わってしまうことに起因する問題がある。この問題に対処するための方法を、図１４を参照して説明する。

図１４は、前述のように説明した、力計測部２１が計測した力Ｆｓをもとに、第二力算出部２３が、操作軸３１に対する操作力を算出する別の方法を説明する図である。図１４においては、力Ｆｓの並進方向の成分の力Ｆが作用する方向と、所定方向への方向の回転操作とをもとにして、操作軸３１の回転中心線回りに作用させる、並進方向の力から成る力を求める。

まず、前述のように、力Ｆｓの並進方向の成分の力Ｆをもとに、平面Ｃ上の、並進方向の力からなる力Ｆｐを算出する。算出された力Ｆｐの方向をもとに、力Ｆｐと直線Ｌｗとが成す角度を求める。この算出された角度と、所定の角度の範囲Ｒｐとを比較する。力Ｆｐの方向が、所定の角度の範囲Ｒｐ以内にあると判定されるとき、力Ｆｐの方向が代表的な方向である所定方向Ｄｎ（図示しない）になるように、点Ｐ２を回転中心点として力Ｆｐを回転させ、それにより、力Ｆｎを算出する。この力Ｆｎの方向は、位置ベクトルＰｖと直交する方向とすることが好ましい。このように求めた、位置ベクトルＰｎと力Ｆｎの外積演算から、力のモーメントを算出し、算出した力のモーメントＭ１３を操作力とする。

このように、力のモーメントを求めるときに、操作軸３１の回転中心線回りに作用する力Ｆｐの方向を、方向に応じて定まる代表的な方向にすることによって、力Ｆｐの方向の変動に起因する、力のモーメントの変動を軽減させ、操作力の変動や、急激な変化などを防ぐことができる。従って、ロボット５０を安定して移動させることができ、操作性を向上させることができる。

また、操作力と力制御ゲインとをもとに、操作軸の移動速度を求める場合、同じ大きさの力を作用させても、力Ｆｐの方向によって、意図せずに、操作軸の移動速度が変わることを防ぐことができる。

また、力Ｆｐの方向を回転させるときに用いる、所定方向Ｄｎを、力Ｆｐの方向に応じて変更するようにしてもよい。所定方向Ｄｎを力Ｆｐの方向に応じて変更して、力Ｆｐの方向を回転させる方法の一例を、図１５を参照しつつ説明する。

図１５に示されるように、所定の角度の範囲として、範囲Ｒｐ１、Ｒｐ２、Ｒｐ３のように順次異なる範囲を設定する。力Ｆｐと直線Ｌｗとが成す角度をもとに、力Ｆｐの方向が、前記角度の範囲Ｒｐ１、Ｒｐ２、Ｒｐ３のどの範囲にあるかを判定する。範囲毎に予め設定した所定方向Ｄｎに基づいて、点Ｐ２を回転中心点として、力Ｆｐの方向を回転させる。

例えば力Ｆｐの方向が範囲Ｒｐ１内にあるとき、力Ｆｐを所定方向Ｄｎに回転させた力Ｆｎ１を力Ｆｎとする。力Ｆｐの方向が範囲Ｒｐ２内にあるとき、力Ｆｐを別の所定方向Ｄｎに回転させた力Ｆｎ２を力Ｆｎとする。さらに、力Ｆｐが範囲Ｒｐ３内にあるとき、力Ｆｐをさらに別の所定方向Ｄｎに回転させた力Ｆｎ３を力Ｆｎとする。このように所定方向Ｄｎを範囲毎に段階的に切替えるときには、操作力が大きく変わらないように、力Ｆｐの方向を円滑に変化させる、または、算出する力のモーメントＭ１３が円滑に変化するようにすることが好ましい。

このように、力Ｆｐの方向を回転させるときに用いる、所定方向Ｄｎを、力Ｆｐの方向をもとに、所定の範囲毎に段階的に変えることによって、力Ｆｐの方向をなるべく考慮して力のモーメントＭ１３が変わるようにできる。さらに、この場合には、力Ｆｐの方向が所定の範囲内にあるときの力Ｆｐの方向の変動による、力のモーメントＭ１３の細かな変動を防ぐことができる。これによって、操作力の変動を減らし、ロボット５０をより安定、安全に移動させ、操作性を向上させることが可能となる。

また、位置ベクトルＰｖの大きさが大きくなるに従って、所定方向Ｄｎと位置ベクトルＰｖとの成す角度が小さくなる方向に所定方向Ｄｎを変更してもよい。このとき、位置ベクトルＰｖの大きさが大きくなるに従って、位置ベクトルＰｖの大きさに対して、所定の範囲毎に段階的に、または、所定の関数に従って段階的に、所定方向Ｄｎと位置ベクトルＰｖとの成す角度が小さくなる方向に所定方向Ｄｎを変更する。

所定方向Ｄｎを範囲毎に段階的に切替えるときには、操作力が大きく変わらないように、力Ｆｐの方向を円滑に変化させる、または、算出する力のモーメントＭ１３が円滑に変化させることが好ましい。

このように、位置ベクトルＰｖの大きさが大きくなるに従って、所定方向Ｄｎと位置ベクトルＰｖとの成す角度が小さくなる方向に所定方向Ｄｎを変更することによって、力Ｆｎが同じ大きさであっても、力のモーメントＭ１３の大きさを小さくして、操作力を小さくすることができる。

これによって、操作力と力制御ゲインとをもとに、操作軸の移動速度を求める場合、力Ｆｐが同じ大きさでも、ロボット５０の先端部５８が操作軸から遠位にあるときには、操作軸の移動速度を小さくできる。この移動速度、つまり、軸の回転速度を小さくした状態で、ロボット５０の先端部５８を移動させることによって、操作者がより安全にロボット５０を移動させることが可能となる。

さらに、図１０を参照した前述の説明と同様に、力のモーメントＭ２１を算出して用いてもよい。つまり、位置ベクトルＰｖの大きさが所定の値より小さいとき、力のモーメントＭ２１を操作力とする。また、位置ベクトルＰｖの大きさが所定の値以上であるとき、力のモーメントＭ１３、または、力のモーメントＭ１３と力のモーメントＭ２１とを足し合わせた力のモーメントを操作力とする。さらに、力のモーメントＭ１３と、力のモーメントＭ２１のそれぞれに係数をかけて大きさを変えることにより、それぞれの影響力を調整した値を、足し合わせるようにしてもよい。このとき、この係数を、位置ベクトルＰｖの大きさや、力Ｆｐの大きさなどをもとに、調整してもよい。

また、前述した、力のモーメントＭ１２の算出時に行った、平面Ｃ上の力Ｆｐが作用する位置ベクトルの伸縮調整の操作と、力のモーメントＭ１２の算出時に行った、力Ｆｓの並進方向の成分の力Ｆの方向および所定方向への回転操作をもとに、力のモーメントを算出するようにしてもよい。

この算出方法の実施の一例を、図１６を参照しつつ、説明する。

図１６は、図１０、図１３、図１４、図１５などと同様に、力計測部２１が計測した力Ｆｓをもとに、第二力算出部２３が、操作軸３１に対する操作力を算出する方法を説明する図である。

前述したのと同様に、点Ｐ１は操作軸３１の基準座標系上の位置を表す原点とする。平面Ｃは、原点が点Ｐ１にあり、Ｚ軸Ａｚが操作軸３１の回転中心線と一致し、Ｘ軸ＡｘおよびＹ軸Ａｙの成す平面が、操作軸３１の回転中心線と直交する平面となるように、操作軸３１に設定した座標系の、Ｘ軸ＡｘおよびＹ軸Ａｙが成す平面であり、Ｘ−Ｙ平面とする。点Ｐ２は、ロボット５０の先端部５８に作用された力を計測するときの力計測座標系の原点である、力計測点を、平面Ｃに射影した点とする。位置ベクトルＰｖは、平面Ｃにおいて、点Ｐ１から点Ｐ２への位置ベクトルとする。力Ｆｐは、力Ｆｓの並進方向の成分の力Ｆをもとに、前述のように、平面Ｃ上で操作軸３１の回転中心線回りに作用する力として求めた並進方向の力である。Ｌｗは、平面Ｃ上に存在し、点Ｐ１および点Ｐ２を含む直線とする。

このとき、並進方向の力からなる力Ｆｐが作用する、位置ベクトルＰｖをもとに、位置ベクトルＰｖの大きさを所定の値Ｃｐｎにまで伸縮調整させた位置ベクトルＰｎを算出する。並進方向の力から成る力Ｆｐが作用する点を、この位置ベクトルＰｎの端点である位置Ｐ４に移動させるように、力Ｆｐを平行移動させた力を、力Ｆｎｐとする。

平面Ｃ上を平行移動させて求められた力Ｆｎｐの方向をもとに、力Ｆｎｐと直線Ｌｗとが成す角度を算出する。この算出した角度と、所定の角度によって与えられる所定の角度の範囲Ｒｐとを比較する。力Ｆｐの方向が、所定の角度の範囲Ｒｐ以内にあると判定されるときには、力Ｆｐの方向を、所定方向に回転させた力Ｆｎを算出する。

このように求めた、位置ベクトルＰｎと力Ｆｎの外積演算から、力のモーメントを算出し、算出した力のモーメントＭ１４を操作力とする。ただし、位置ベクトルＰｖの大きさが所定の値より小さいときは、前述したのと同様に、力のモーメントＭ２１を、操作力とする。

このように、操作軸３１の回転中心線回りに作用する力Ｆｐの方向を方向に応じて定まる代表的な方向にする、また、力が作用する位置ベクトルを所定の大きさにする。これにより、力のモーメントを求めるときに、点Ｐ２の位置の変動による、また、力Ｆｐの方向の変動よる、力のモーメントの変動を軽減させ、操作力の細かな変動や、急激な変化などを防ぐことができる。従って、ロボット５０を安定して移動させることができ、操作性を向上させることができる。

また、操作力と力制御ゲインとをもとに、操作軸の移動速度を求める場合、同じ大きさの力を作用させても、力Ｆｐの方向によって、意図せずに、操作軸の移動速度が変わることを防ぐことができる。

また、力Ｆｐの方向の変更、力Ｆｐが作用する位置ベクトルの大きさを所定の値にする方法の別の実施例、力のモーメントＭ２２を考慮する方法などは、前述したのと同様である。このようなことによって、ロボット５０に力を作用させて移動させるときの、操作性を向上させるとともに、より安全にすることが可能である。

操作力の算出方法として、別の実施例について説明する。

力計測部２１が計測した力Ｆｓの並進方向の成分の力Ｆをもとに、操作軸３１の回転中心線回りに作用させる並進方向の力から成る力Ｆｐを求め、前記並進方向の力から成る力Ｆｐの方向をもとに、操作力の方向を求める。もしくは、力計測部２１が計測した力Ｆｓの並進方向の成分の力Ｆの大きさ、または、操作軸３１の回転中心線回りに作用させる力Ｆｐの大きさをもとに、操作力の大きさを求めるようにしてもよい。

この実施例について、図１７を参照しつつ、さらに説明する。

前述の実施例と同様に、点Ｐ１は操作軸３１の基準座標系上の位置を表す原点とする。平面Ｃは、原点が点Ｐ１にあり、Ｚ軸Ａｚが操作軸３１の回転中心線と一致し、Ｘ軸ＡｘおよびＹ軸Ａｙの成す平面が、操作軸３１の回転中心線と直交する平面となるように、操作軸３１に設定した座標系の、Ｘ軸ＡｘおよびＹ軸Ａｙが成す平面であり、Ｘ−Ｙ平面とする。点Ｐ２は、ロボット５０の先端部５８に作用させた力を計測するときの力計測座標系の原点である、力計測点を平面Ｃに射影した点とする。位置ベクトルＰｖは、平面Ｃにおいて、点Ｐ１から点Ｐ２への位置ベクトルとする。力Ｆｐは、力Ｆｓの並進方向の成分の力Ｆをもとに、前述のように、平面Ｃ上で操作軸３１の回転中心線回りに作用する力として求めた並進方向の力である。Ｌｗは、平面Ｃ上に存在し、点Ｐ１および点Ｐ２を含む直線とする。

位置ベクトルＰｖの大きさが所定の値より小さいとき、操作力は作用しないとする。そして、位置ベクトルＰｖの大きさが所定の値以上であるとき、以下のようにする。

まず、力Ｆｐをもとに、操作軸３１に対して仮想的に作用するとした仮想力を次のように求める。力Ｆｐのベクトルと、位置ベクトルＰｖの内積を求めるなどして、力Ｆｐの方向が、直線Ｌｗと直交する正方向（＋Ｒｑの方向）であるか、直線Ｌｗと直交する負方向（−Ｒｑの方向）であるかによって、仮想力の方向を求め、仮想力の符号を決める。図１７に示す場合には、力Ｆｐの方向は、直線Ｌｗと直交する＋方向（＋Ｒｑの方向）にあると求められる。従って、この方向をもとに、仮想力の方向を正方向とし、仮想力の符号を正とする。

さらに、力Ｆｐの大きさをもとに、または、力Ｆｓの並進方向の成分の力Ｆの大きさを、仮想力の大きさとする。または、力Ｆｐの位置ベクトルＰｖと直交する成分の大きさを、仮想力の大きさとしてもよい。このようにして求めた仮想力Ｆｋを、操作力とする。

さらに、力計測部２１が計測した力Ｆｓのモーメント成分の力Ｍをもとに求めた、前記力のモーメントＭ２１を算出して用いてもよい。位置ベクトルＰｖの大きさが所定の値より小さいとき、力のモーメントＭ２１を操作力とする。

また、位置ベクトルＰｖの大きさが所定の値以上であるときには、前記仮想力Ｆｋに所定の値をかけた値、または、前記仮想力Ｆｋに所定の値をかけた値と力のモーメントＭ２１とを足し合わせた力のモーメントを操作力とする。さらに、前記仮想力Ｆｋに所定の値をかけた値と、力のモーメントＭ２１のそれぞれに係数をかけて大きさを変えることにより、それぞれの影響力を調整した値を、足し合わせるようにしてもよい。このとき、この係数を、位置ベクトルＰｖの大きさや、力Ｆｐの大きさなどをもとに、調整してもよい。

前述の説明と同様に、力計測部２１が計測した力Ｆｓのモーメント成分の力Ｍの影響を排除するようにしたい場合や、力Ｆｓの並進方向の力のみで移動させたい場合には、力のモーメントＭ２１を考慮せず、仮想力Ｆｋをもとにした力を操作力とすることが好ましい。

操作指令部２４は、制御モードが第二制御モードのとき、前述のようにして求めた操作力と操作軸設定部２５によって設定される力の方向に応じて定まる移動方向に基づいて操作軸を移動させる。このとき、操作指令部２４は、操作力の符号と、操作軸設定部２５が設定した力の方向に応じて定まる、ここでは、操作力の符号に応じて定まる操作軸の移動方向をもとに、操作軸の目標移動方向（回転軸の場合、回転方向）を決定し、操作力の大きさをもとに、操作軸の目標移動速度を算出する。

このとき、操作力の大きさに、力に対する移動の応答性を定める力制御ゲインをかけた力制御を行うことによって、操作軸の目標移動速度を算出することが好ましい。また、操作軸の回転中心線からロボット５０の先端部５８までの最短距離に応じて、前記力制御ゲインを変更するようにしてもよい。

このとき、操作軸の回転中心線からロボット５０の先端部５８までの最短距離をもとに、所定の範囲毎に段階的に、または、所定の関数に従って段階的に、または、所定の関数に従って連続的に、力制御ゲインを変更する。なお、力制御ゲインを段階的に切替えるときには、ロボット５０の移動速度が急に、大きく変わらないように、移動の速度を円滑に変化させることが好ましい。

これによって、ロボット５０の先端部５８の位置によって、操作力に対する応答性を変更し、空間における領域毎に、ロボット５０の移動速度を調整することができる。

ここで、操作軸の回転中心線からロボット５０の先端部５８までの最短距離が大きい場合と、小さい場合とにおいて、同じ操作力に対して、操作軸の速度を同じ角速度で移動させることを考える。最短距離が大きい場合には、最短距離が小さい場合よりも、ロボット５０の先端部５８の並進方向の速度は速くなる。

そして、操作軸からロボット５０の先端部５８の位置が遠い場合には、操作軸からロボット５０の先端部５８の位置が近い場合よりも、同じ操作力に対する操作軸の角速度を遅くした方が、安全であり、また操作しやすくなる。従って、操作軸の回転中心線からロボット５０の先端部５８までの最短距離が大きくなるに従って、力制御ゲインを小さくするようにしてもよい。

これによって、操作力の大きさが同じ場合でも、操作軸からロボット５０の先端部５８の位置が遠くなるに従って、ロボット５０の先端部５８の速度を小さくすることができる。その結果、安全性を高め、操作性を向上させることが可能となる。

また、操作力の大きさに応じて操作軸の目標移動速度を変える場合には、操作力の大きさが同じでも、ロボット５０の先端部５８が、操作軸から遠くなるに従って、ロボット５０の先端部５８の接線速度は大きくなってしまう。

そこで、操作指令部２４は、制御モードが第二制御モードのとき、前述のようにして求めた操作力と、操作軸設定部２５によって設定される力の方向に応じて定まる移動方向とに基づいて、操作軸を移動させるとき、操作力に基づいて、操作軸の回転中心線回りの、ロボット５０の先端部５８の目標移動方向および目標接線速度を求める。そして、操作指令部２４は、ロボット５０の先端部５８の目標移動方向および目標接線速度をもとに、操作軸の目標移動方向および目標移動速度を求めて、前記操作軸を移動させるようにしてもよい。

これによって、操作力の大きさが同じ場合、ロボット５０の先端部５８の位置によらず、ロボット５０の先端部５８の接線速度を同じにすることが可能となる。この場合、ロボット５０の先端部５８が、操作軸から離れるに従って、操作力の大きさが同じでも、操作軸の回転速度は小さくなる。

また、このような効果を得るため、操作力の大きさをもとに、操作軸の目標移動速度を算出する場合には、前述したように、操作軸の回転中心線からロボット５０の先端部５８までの最短距離が大きくなるに従って、力制御ゲインを小さくしてもよい。また、前述した効果を得るために、操作力を算出するときに、操作軸の回転中心線からロボット５０の先端部５８までの最短距離で除算演算して小さくなるように求めてもよい。

また、操作力に基づいて、操作軸の回転中心線回りのロボット５０の先端部５８の移動方向および接線速度を求めた後で、操作軸の目標移動方向および目標移動速度を求めるとき、操作力の大きさに、力に対する移動の応答性を定める力制御ゲインをかけた力制御によって、ロボット５０の先端部５８の目標接線速度を算出してもよい。このときにも、操作軸の回転中心線からロボット５０の先端部５８までの最短距離に応じて、力制御ゲインの値を変更するようにしてもよい。

このような場合には、操作軸の回転中心線からロボット５０の先端部５８までの最短距離をもとに、所定の範囲毎に段階的に、または、所定の関数に従って段階的に、または、所定の関数に従って連続的に、力制御ゲインを変更する。なお、力制御ゲインを段階的に切替えるときには、ロボット５０の移動速度が急に、大きく変わらないように、移動の速度を円滑に変化させることが好ましい。

また、前述の場合に、操作軸の回転中心線からロボット５０の先端部５８までの最短距離が大きくなるに従って、力制御ゲインを小さくするようにしてもよい。これによって、操作軸の回転中心線から、ロボット５０の先端部５８の位置が遠ざかるに従って、操作軸の回転中心線回りの、ロボット５０の先端部５８の接線速度を小さくすることができ、ロボット５０をより安全に操作することが可能となる。

以上のようにして、操作指令部２４は、制御モードが第二制御モードのとき、操作軸設定部２５の設定と、第二力算出部２３が、算出する操作力をもとに、操作軸としたロボット５０の所望の軸を移動させる指令を出力する。

本発明の第一実施形態に係るロボット制御装置１０を実現するロボット制御装置１０ａでは、ロボット５０の先端部５８に力を作用させてロボット５０を移動させる際、直交座標系上のロボット５０の先端部５８の位置および／または姿勢の移動をさせているときの、特異姿勢の近傍では、ロボット５０の各軸の位置を移動させる。また、制御周期毎のロボット５０の先端部５８の位置および／または姿勢の制御を行うか、または、制御周期毎のロボット５０の所望の各軸の位置の制御を行うかを、特異姿勢近傍判定部２７を用いて判定して切替える。このため、本発明においては、以下のような効果がある。

ロボット５０が特異姿勢の近傍にある場合に、通常の直交座標系上での動作では移動させることができない状態や、直交座標系上の制御性能が悪化する状態、または、制御ができなくなる状態が存在する。本発明では、そのような状態を検出し、操作力に応じて各軸の位置を制御する動作に切替えることによって、ロボットを特異姿勢の近傍を安定的に通過させることが可能となる。

そして、ロボット５０の先端部５８に作用させる力に応じて、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させる操作では移動させることができないような各軸の位置に、移動させることが可能となる。

また、特異姿勢の近傍において、所定の軸や、所望の軸について、各軸の移動操作を可能とするとともに、適切な軸を操作する軸として選定することによって、各軸の移動操作をより簡易にできる。

また、特異姿勢の近傍はロボット５０の軸の位置を定める形態が変わる場所でもある。このため、特異姿勢の近傍で各軸を移動させることによって、直交座標系上の位置および／または姿勢の移動では移動できないような位置を通過させて、別の形態に移動させることができる。なお、ここで述べるロボットの「形態」とは、ロボットの先端部を或る位置および姿勢にするときに、軸の位置が一意に定まらず、軸の位置をどのようにするかという軸の状態を定める形態とする。そして、ロボット５０の先端部５８に力を作用させて、直交座標系上でロボット５０の先端部５８の位置および／または姿勢を移動させているときに、一旦、特異姿勢の近傍に移動させて、各軸を移動させることによって、別の形態での直交座標系上の移動操作を行うことができる。
また、特異姿勢の近傍やその手前では、直交座標系上の位置を少し移動させただけでも、回転軸が意図せずに大きく回転してしまうことがある。このときに、特異姿勢の近傍で各軸の移動操作に切替え、意図せずに移動した各軸をもとの所望の位置にすることができる。

ロボット５０の先端部５８に力を作用させるという同様の操作によって、直交座標系上の移動操作と、各軸を移動させる移動操作とを切替える。これによって、ロボット５０の姿勢を、より任意の姿勢へ移動しやすくする。

また、操作軸設定部２５が、特異姿勢の近傍にあることの要因となる軸または特異姿勢の近傍を通過させる軸を含む軸を操作軸とすることにより、各軸の制御を行う制御モードから、直交座標系上の制御を行う制御モードへの切替えが容易となる。

そして、本発明においては、特異姿勢の近傍という状態を利用して、制御モードを切替えている。このため、直交座標系上の制御を行う制御モードと、各軸の位置を動かす制御モードとを切替えるための、ロボット５０の各軸の位置の状態、およびタイミングが分かりやすく、制御モードの切替えの操作が容易となる。

図１８は、本発明の第二実施形態に係るロボット制御装置１０ｂの構成を機能的に示す図である。図示されるように、ロボット制御装置１０ｂは、力計測部２１と、第一力算出部２２と、第二力算出部２３と、操作指令部２４と、操作軸設定部２５と、記憶部２６と、軸位置状態判定部２８と、表示出力部７１と、を具備している。

ロボット制御装置１０ｂは、ロボット制御装置１０ａの特異姿勢近傍判定部２７の代わりに、軸位置状態判定部２８を用いて、ロボット５０の先端部５８の位置および／または姿勢を制御する第一制御モードと、ロボット５０の軸の位置を制御する第二制御モードとを切替えるように形成されている。

また、操作軸設定部２５、操作指令部２４は、特異姿勢近傍判定部２７に代えて、軸位置状態判定部２８を用いるように構成されている。ロボット制御装置１０ｂの構成および作用のうち、本発明の１番目の実施形態に係るロボット制御装置１０として、図２に関連してロボット制御装置１０ａについて説明された実施形態と重複するものについては説明を省略する。

軸位置状態判定部２８は、ロボット５０の複数の軸のうち、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあるか否かの判定を行う。例えば、ロボット５０の一つまたは複数の軸について、軸毎の位置が、前記軸毎に設定された所定の位置から閾値以内にあるか否かという、所定の位置関係の条件を満たすかどうかを判定する。または、複数の軸の位置関係によって、基準座標系上の、或る軸の原点の位置が、所定の位置関係の条件を満たすかどうかを判定する。このような軸位置状態判定部２８を特異姿勢近傍判定部２７として用いることにより、軸位置の状態判定によって特異姿勢の近傍にあるか否かの判定を行うようにしてもよい。

この場合の実施例は、特異姿勢近傍判定部２７の実施例として示したものと同様である。つまり、特異姿勢の状態、分類毎に、各軸の位置が、前記軸毎に設定された所定の位置から閾値以内にあるか否かという、所定の位置関係の条件を満たすかどうかという判定を行う。または、複数の軸の位置関係によって、基準座標系上における或る軸の原点の位置が、所定の位置関係の条件を満たすかどうかを判定して、ロボット５０の軸の位置が所定の状態にあるか否かの判定を行う。

ロボット５０の所定の軸の位置が特異姿勢の近傍のような状態にない場合であっても、軸位置状態判定部２８は、ロボット５０の先端部５８の位置および／または姿勢を制御するときに、ロボット５０の先端部５８の位置を通常は移動させないような状態、または、そのような状態に移動させる頻度が少ない状態にあるか否かを判定するようにしてもよい。

軸位置状態判定部２８が、このような状態の軸の位置にあることを検出する。この場合には、後述のように、操作指令部２４が、ロボット５０の先端部５８の位置および／または姿勢を制御する前記第一制御モードと、ロボット５０の軸の位置を制御する前記第二制御モードとを、ロボット５０の移動操作中に容易に切替えることができるようになる。

軸位置状態判定部２８によって一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあると判定されるとき、操作軸設定部２５は、ロボット５０の現在の各軸の位置をもとに、所定の位置関係の条件を満たす状態の位置にあることの要因となる軸、または、所定の位置関係の条件を満たす状態の位置を通過させる軸、を含む一つ乃至複数の所定の軸を、力に応じて移動させる操作軸として設定する。また、力の方向に応じて定まる操作軸の移動方向を設定する。

このとき、力の方向に応じて定まる操作軸の移動方向は、予め設定された値に基づいて、または、特異姿勢の近傍になるときの操作軸の移動方向と、操作軸にかかる力の方向とに基づいて設定される。

複数の軸の内から軸を選択して操作軸を設定するとき、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあることの要因となる軸、または、その状態の位置を通過させる軸以外の軸においては、そのときのロボット５０の各軸の位置をもとに、所定の軸を操作軸として選択する。

操作指令部２４は、ロボット５０の先端部５８に作用された力に基づいてロボット５０を移動させるように、力計測部２１が計測した力をもとに、第一力算出部２２または第二力算出部２３が算出した操作力を用いて、ロボット５０を移動させる操作指令を出力する。

操作指令部２４は、ロボット５０を移動させる制御モードとして、ロボット５０を移動させる、第一制御モードと第二制御モードとを有する。軸位置状態判定部２８によって、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にない、と判定されるとき、操作指令部２４は、ロボット５０を移動させる制御モードを、第一制御モードに設定する。そして、軸位置状態判定部２８によって、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあると判定されるとき、操作指令部２４は、ロボット５０を移動させる制御モードを、第二制御モードに設定する。そして、設定された制御モードに基づいて、ロボット５０を移動させる。

次いで、操作者６０がロボット５０の先端部５８に力を作用させてロボット５０を移動させるときの、本発明の第二実施形態に係るロボット制御装置１０ｂによる処理の過程の一例を図１９を参照して説明する。図１９は、ロボット制御装置１０ｂによる処理の過程の一例を示すフローチャートである。

以下、ロボット５０の先端部５８に力を作用させてロボット５０を移動させるときの、ロボット制御装置１０ｂによる制御モードの切替えの実行処理を、図１９のフローチャート及び関連する図面を参照しつつ、また、制御モードが第二制御モードのときのロボット制御装置１０ｂによる実行処理を、図８のフローチャート及び関連する図面を参照しつつ説明する。

なお、ここで説明される一連の処理は、一例であって、本発明がこの具体例に限定されるわけではないことに留意されたい。図１９は、制御モードを設定する処理の一例を示す図である。

ロボット５０を移動させる処理が開始されると、軸位置状態判定部２８により、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあるか否かの判定を行う（ステップＳＡ１）。そして、この軸位置状態判定部２８を用いて、制御周期毎の直交座標系上のロボット５０の先端部５８の位置および／または姿勢の制御を行うか、または、制御周期毎の所望の各軸の位置の制御を行うかを判定する。

軸位置状態判定部２８によって、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にない、と判定されるとき、制御モードを第一制御モードとする（ステップＳＡ２）。

軸位置状態判定部２８によって、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にある、と判定されるとき、制御モードを第二制御モードとする（ステップＳＡ３）。

次に、制御モードに応じて、ロボット５０を移動させる操作指令を変更する。この方法は、前述したように、制御モードが第一制御モードの処理について、図７を参照しつつ説明した処理、また、制御モードが第二制御モードの処理について図８を参照しつつ説明した処理と、同様である。

このように、軸位置状態判定部２８を用いて、ロボット５０の軸の位置をもとに、直交座標系上のロボット５０の先端部５８の位置および／または姿勢の移動を行う第一制御モードと、ロボット５０の各軸の位置の移動を行う第二制御モードとを切替えることができ、また各軸を操作する場合の軸を指定することができる。このため、ロボット５０の先端部５８に力を作用させて移動させているときに、別途用意した入力装置を用いて入力作業を行うことなしに、制御モードを切替えることが可能となる。

また、ロボット５０の先端部５８に力を作用させるという操作を行いながら、直交座標上の位置および／または姿勢を移動させたり、所望の各軸を移動させたりすることが可能となる。例えば、ロボット５０の先端部５８に力を作用させて、直交座標系上のロボット５０の先端部５８の位置および／または姿勢を移動させているときに、ロボット５０の先端部５８に力を作用させながら所望の各軸を移動させて、直交座標系上の通常の移動では移動させられない位置を通過させ、或る位置から通常、移動可能でない位置に到達させられる。その後、直交座標系上の移動を再度行うことなどが可能となる。

これによって、操作者が設定を切替える教示装置を別に用意したり、設定を切替えるための入力操作をしたりすることがない。従って、ロボットシステムのコストを抑え、移動操作において、直交座標系上の任意の位置や姿勢、また、任意の軸の位置に移動させやすくなるとともに、よりスムーズに連続的に且つ快適に行うことが可能となる。

制御モードが第二制御モードの場合における、各軸の位置を移動させる処理の実施例についてさらに説明する。軸位置状態判定部２８が、一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあると判定したときに、図８のステップＳ２２において、操作軸設定部２５が、各軸の現在位置をもとに、操作軸、および、力の方向に応じて定まる操作軸の移動方向を設定する方法について説明する。そして、同様な場合において図８の、ステップＳ２３において、第二力算出部２３によって操作軸を移動させるための操作力を算出する方法、およびステップＳ２４において操作指令部２４によって移動の操作指令を生成する方法についてさらに説明する。

まず、軸位置状態判定部２８によって、所定の軸をＪ５軸５５として、Ｊ５軸５５の位置が所定の閾値以内であるか否かを判定させるようにする。この所定の閾値は、ロボット５０の先端部５８の位置および／または姿勢を、通常は移動させないような値である。例えば、図１において、ロボット５０の先端部５８やフランジ部５７が、Ｊ４軸５４とＪ５軸５５を繋ぐリンクに、通常、移動させない位置まで近づく状態の位置、または、Ｊ３軸５３の原点、Ｊ５軸５５の原点、およびフランジ部５７の原点が直線上およびその近傍に存在する状態（この位置を、ここでは、Ｊ５軸５５の位置が０度とする）の位置とする。ここでは、Ｊ５軸５５が０度から閾値以内にあるか否かを判定し、前記閾値以内にあるとき、所定の位置関係の条件を満たす状態にあるとする。

操作軸設定部２５は、Ｊ５軸５５が０度から閾値以内にある、所定の位置関係の条件を満たす状態のとき、操作軸を次のように設定する。所定の位置関係の条件を満たす状態の位置にあることの要因となる軸、または、所定の位置関係の条件を満たす状態の位置を通過させる軸である、Ｊ５軸５５を含むように、複数の軸の内から軸を選定して、操作軸を設定する。

また、各軸の現在位置をもとに、操作軸に設定する軸として、所定の位置関係の条件を満たす状態の位置にあることの要因となる軸、または、所定の位置関係の条件を満たす状態の位置を通過させる軸以外の軸も選定する。ここでは、Ｊ４軸５４の位置に応じて、Ｊ５軸５５以外の軸も操作軸に設定する。

これは次のことによる。Ｊ５軸５５の回転中心線回りに作用する並進方向の力をもとに、操作力を求める場合には、ロボット５０の先端部５８に作用される並進方向の力の方向が、Ｊ５軸５５の回転中心線と平行な方向であるとき、操作力はほぼ作用しないようになる。

従って、Ｊ５軸５５の回転中心線と直交する方向に近い、並進方向の力を作用させたとき、Ｊ５軸５５を移動させるようにする。そして、並進方向の力の方向が、それ以外の方向の場合、異なる軸を移動させるようにする。

Ｊ４軸５４の位置が変わると、Ｊ５軸５５の回転中心線が、Ｊ１軸５１軸の回転中心線と成す角度、Ｊ２軸５２の回転中心線と成す角度、Ｊ３軸５３の回転中心線と成す角度は変化する。この状態に応じて、Ｊ１軸５１、Ｊ２軸５２およびＪ３軸５３の内から、操作軸として設定するものを切替えるようにするため、Ｊ４軸５４の位置を利用してどの軸を操作軸とするかを判定する。

Ｊ４軸５４の位置が、Ｊ５軸５５の回転中心線と、Ｊ１軸５１の回転中心線との成す角度が９０度から閾値以内となるような位置の場合、Ｊ５軸５５以外の操作軸としてＪ１軸５１を操作軸とする。

Ｊ４軸５４の位置が、Ｊ５軸５５の回転中心線と、Ｊ２軸５２の回転中心線との成す角度が９０度から閾値以内となるような位置の場合、Ｊ５軸５５以外の操作軸としてＪ２軸５２を操作軸とする。Ｊ４軸５４の位置が、Ｊ５軸５５の回転中心線と、Ｊ３軸５３の回転中心線との成す角度が、前記のＪ２軸５２の場合とは逆方向の９０度から閾値以内となるような位置の場合、Ｊ５軸５５以外の操作軸としてＪ３軸５３を操作軸とする。

Ｊ４軸５４の位置が、前記条件を満たさない位置である場合、Ｊ５軸５５以外の操作軸としてＪ４軸５４を操作軸とする。これによって、Ｊ４軸５４の位置を、前述した、Ｊ１軸５１、Ｊ２軸５２およびＪ３軸５３を操作軸とする位置に移動させることができる。

なお、この場合、力を作用させる位置が、Ｊ４軸５４の回転中心線上に在る場合には、操作力を力のモーメントとして移動させる。力を作用させる位置が、Ｊ４軸５４の回転中心線上にない場合には、操作力を、並進方向の力、力のモーメントの両方として移動させる。

また、前述した場合において、Ｊ５軸５５の回転中心線とＪ１軸５１の回転中心線との成す角度が前記の場合とは逆の９０度から閾値以内となるような位置にＪ４軸５４が位置する場合、Ｊ５軸５５以外の操作軸として、別の軸、例えば、Ｊ４軸５４を、操作軸としてもよい。

このように、所定の位置関係の条件を満たす状態のとき、所定の位置関係の条件を満たす状態の位置にあることの要因となる軸、または、所定の位置関係の条件を満たす状態の位置を通過させる軸、を含む複数の軸を操作軸に設定する。なお、このような方法を、ロボット制御装置１０ａの実施方法において適用してもよい。

以上のようにして、操作軸を選定し、操作軸における力の方向に応じて定まる移動方向は、この場合、いずれの軸についても、力の方向と同じ方向とする。ステップ２３において、前記操作軸設定部２５が設定した操作軸に対して、第二力算出部２３が操作力を算出する方法は、前記ロボット制御装置１０ａの場合の方法と同様とする。

ステップ２４において、操作指令部２４は、操作軸設定部２５が設定した操作軸の位置を、第二力算出部２３が算出した操作力と、操作軸設定部２５が設定した力の方向に応じて定まる移動方向とに基づいて、前記ロボット制御装置１０ａの場合の方法と同様に移動させる操作指令を生成する。

本発明の３番目の実施形態においては、ロボット制御装置１０における、操作軸設定部２５は、さらに、直交座標系上の位置および／または姿勢の移動を行う第一制御モードから、各軸の位置の移動を行う第二制御モードになる直前または第二制御モードになったときの、操作軸とする軸にかかる力の方向と、前記操作軸とする軸の移動方向とをもとに、前記操作軸にかかる力の方向に応じて定まる移動方向を設定することが好ましい。

第一制御モードから第二制御モードに入る前の移動動作を考慮しない場合、操作上の違和感、圧迫感、困難があるだけではなく、ロボット５０の先端部５８の直座標系上の移動操作によっては、第二制御モードのときの位置と、第一制御モードのときの位置との境界の部分で、制御モードに関する状態が繰返し入替わることにより、不安定な動作をしてしまうことがある。

第一制御モードにおける或る操作軸について、その操作軸にかかる力の方向とロボット５０の移動方向とが逆方向である状態から、第二制御モードに切替わる場合を考える。この場合には、第一制御モードから第二制御モードに切替わったとき、力の方向と移動方向の関係を直前の状態と同じようにしなければ、第二制御モードで移動させた途端に、第一制御モードに戻ってしまう可能性がある。

このことに対処するために、第一制御モードから第二制御モードに切替えるときには、第二制御モードに切替わるときやその直前の、移動動作や、操作軸とする軸の移動方向、操作軸とする軸に作用する力の方向をもとに、操作軸の力の方向に応じて定まる移動方向を設定することが好ましい。これによって、第一制御モードの状態から第二制御モードの状態へ移行して通過させるときの操作上の違和感、圧迫感、困難がなく、ロボットを継続的に移動させることが可能となる。

図１に示される構成のロボットでは、直交座標系上のロボット５０の先端部５８の位置を並進移動させているときに、Ｊ３軸５３の原点、Ｊ５軸５５の原点、フランジ部５７の原点が直線上およびその近傍に存在する状態になる場合、前述した状況になることがある。

このとき、前述した状態になる前にまたは前述した状態の近傍になったときの、Ｊ５軸５５の移動方向とＪ５軸５５にかかる力の方向とをもとに、力の方向に応じて定まる操作軸の移動方向を設定する。

もしくは、前述した状態になる前に、直交座標系上のロボット５０の先端部５８の移動操作を行っていたときは、移動操作が並進方向である場合には、操作軸の移動方向は力の方向とは逆方向にする。また、移動操作が回転方向の操作である場合には、操作軸の移動方向は力の方向と同じ方向としてもよい。これによって、前述した状態の近傍になる前に行っていた直交座標系上の移動操作と、力の方向に応じた、操作軸の移動方向を同じにして、操作性をよくすることが可能となる。

このようにして、所定の状態や、特異姿勢の近傍に入る前の移動動作を考慮することによって、前述した状態への移行、通過において、操作上の違和感、圧迫感、困難を与えることなく、移動操作を円滑に行うことが可能となる。

本発明の４番目の実施形態においては、ロボット制御装置１０における、操作指令部２４は、さらに、第二制御モードにおいて、一つ乃至複数の操作軸の内、軸にかかる力の方向とは逆方向に移動させる操作軸の原点の位置を、前記操作軸の動作によってロボット５０の先端部５８の位置が移動する方向とは逆方向の成分を含む方向に移動させる、または、前記操作軸にかかる力の方向の成分を含む方向に移動させることが好ましい。

このような移動操作の一実施例を述べる。図１に示す構成のロボット５０では、直交座標系上のロボット５０の先端部５８の位置を並進移動させているときに、Ｊ３軸５３の原点、Ｊ５軸５５の原点、フランジ部５７の原点が直線上およびその近傍に存在する所定の状態の位置になる場合、制御モードを第二制御モードに切替え、操作軸をＪ５軸５５として、操作軸を、操作軸にかかる力の方向とは逆方向に移動させる。

このとき、Ｊ５軸５５の軸だけを動かすと、ロボット５０の先端部５８において、力が作用される部分が、力を作用させた方向とは逆方向に移動する。このため、操作者は操作上の違和感や、圧迫感、困難を感じることがある。これを軽減させるため、Ｊ５軸５５を移動させる際、同時に、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を操作軸として移動させる。このとき、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を移動させることによって、Ｊ５軸５５の原点の位置を、Ｊ５軸５５の動作によってロボット５０の先端部５８が移動する方向とは逆方向の成分を含む方向に移動させる。または、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を移動させることによって、Ｊ５軸５５の原点の位置を、Ｊ５軸５５にかかる力の方向の成分を含む方向に移動させる。これによって、力を作用させた方向にＪ５軸５５の原点の位置が移動するために、力を作用させた方向とは逆方向にロボット５０の先端部５８が移動する移動量を小さくすることができる。また、力を作用させた方向と近い方向にロボット５０の先端部５８を移動させることができる。

ただし、軸にかかる力の方向とは逆方向に移動させる操作軸としたＪ５軸５５の原点の位置の移動量が大きい場合、操作軸であるＪ５軸５５の各軸の位置を移動させても、Ｊ５軸５５の原点の移動に伴って、Ｊ５軸５５部分を挟むリンクが成す角度の変化量が小さくなり、Ｊ３軸５３の原点、Ｊ５軸５５の原点、フランジ部５７の原点が直線上およびその近傍に存在する所定の状態の位置、を通過して抜けるのに時間がかかるようになってしまう。場合によっては、前記所定の状態の位置、を通過して抜けることができなくなることもある。このような状況に陥らないように、軸にかかる力の方向とは逆方向に移動させる操作軸としたＪ５軸５５の原点の位置を移動させる移動量は、Ｊ５軸５５の軸の回転移動動作によってロボット５０の先端部５８の位置が移動する分と、さらに、その移動量に所定の係数を値をかけた程度にする。または、Ｊ５軸５５の動作によってロボット５０の先端部５８の位置が移動する移動量に、所定の係数をかけた移動量にすることが好ましい。

また、第二制御モードにおいて、操作軸が、力を作用させる方向とは逆方向に移動されることによる、操作上の違和感や、圧迫感、困難を軽減させる、別の実施例を述べる。Ｊ２軸５２およびＪ３軸５３の両方の軸の回転中心線は、平行かつ同じ方向を向いているとする。このとき、Ｊ２軸５２を、作用させる力の方向とは逆方向に移動させる操作軸としたとき、Ｊ３軸５３を力の方向と同じ方向に移動させる操作軸として動かすようにしてもよい。Ｊ３軸５３をＪ２軸５２と同時に移動させることによって、作用させる力の方向とは逆方向に移動させるＪ２軸５２の原点の位置を、Ｊ２軸５２の動作によってロボット５０の先端部５８の位置が移動する方向とは逆方向の成分を含む方向に移動させる、または、Ｊ２軸５２にかかる力の方向の成分を含む方向に移動させることができる。もしくは、Ｊ３軸５３を、作用させる力の方向とは逆方向に移動させる操作軸としたとき、Ｊ２軸５２を力の方向と同じ方向に移動させる操作軸として動かすようにしてもよい。Ｊ２軸５２をＪ３軸５３と同時に移動させることによって、作用させる力の方向とは逆方向に移動させるＪ３軸５３の原点の位置を、Ｊ３軸５３の動作によってロボット５０の先端部５８の位置が移動する方向とは逆方向の成分を含む方向に移動させる、または、Ｊ３軸５３にかかる力の方向の成分を含む方向に移動させることができる。これによって、ロボット５０の先端部５８を力の方向とは逆方向に移動させる量を小さく、または、ロボット５０の先端部５８を力の方向と同じ方向に移動させることができる。このため、操作上の違和感や、圧迫感、困難を軽減させることができる。以上、前述したようにすることによって、第二制御モードで、操作軸を移動させるときの操作性を向上させることが可能となる。

ところで、図２２Ａから図２２Ｄはロボット５０の部分拡大図である。これら図面においては、操作者６０がロボット５０の先端部５８に力Ｆ１を矢印方向に作用させて、ロボット５０を移動させることを示している。このとき、ロボット５０の先端部５８は、時計回り方向にＪ５軸５５回りに回転移動をしている。移動動作の詳細については、以下で述べる。

図２２Ａにおいては、作用される力に応じて直交座標系上の位置および／または姿勢の移動を行う第一制御モードで、ロボット５０の先端部５８をＪ５軸５５回りに回転移動させていることが示されている。このとき、Ｊ５軸５５回りに回転移動させているので、Ｊ５軸５５のみが移動している。このような回転移動により、図２２Ｂに示されるように、リンク８２（Ｊ３軸５３およびＪ４軸５４と、Ｊ５軸５５とをつなぐリンク）およびリンク８３（Ｊ５軸５５およびＪ６軸５６からフランジ部５７につながるリンク）が直線をなすように互いに接近する。

本発明においては、図２２Ｂにおいて示される状態において、リンク８２およびリンク８３が直線をなすように近づいたときに、第一制御モードから第二制御モードへの切替えを行っている。この理由は、本発明のロボットにおいてＪ５軸５５の上記部分が直線に近い場合にはロボット５０が特異姿勢近傍になって、第一制御モードでの移動を実行できないためである。

第二制御モードのとき、またはその直前において、操作される軸における第二力算出部２３が算出した力と、各軸の移動方向とをもとに、操作軸設定部２５は第二制御モードのときの力の方向に応じて定まる移動方向を設定する。図２２Ａから分かるように、この場合には、Ｊ５軸５５回りの力のモーメントの向きと、Ｊ５軸５５の移動方向は同じ向きである。

そして、図２２Ｃに示されるように、リンク８２およびリンク８３が直線上およびその近傍に存在するときには、第二制御モードのままである。次いで、図２２Ｄに示されるように、回転移動が進んで、リンク８２およびリンク８３が直線をなさないようになると、第二制御モードから第一制御モードに切替わる。

このように図２２Ａから図２２Ｄに示される場合においては、操作者６０がロボット５０の先端部５８に力Ｆ１を作用させて移動させるときに、第一制御モードから第二制御モードに切替え、再度、第一制御モードに戻している。この動作の間、力Ｆ１の方向とＪ５軸５５の回転移動方向とは同じ方向である。言い換えれば、力Ｆ１をかけた方向にＪ５軸５５は移動するので、第二制御モードの状態を円滑に通過することができる。

なお、図２３Ａから図２３Ｄには、操作者６０がロボット５０の先端部５８に力Ｆ２を矢印方向に作用させて、反時計回り方向にＪ５軸５５回りに回転移動させることが示されている。この場合、図２３Ａでは第一制御モード、図２３Ｂおよび図２３Ｃでは第二制御モード、図２３Ｄでは第一制御モードで、移動させる。このような場合にも、図２２Ａから図２２Ｄに示される場合と同様な効果が得られるのは明らかであろう。

ところで、図２４Ａから図２４Ｄはロボット５０の部分拡大図である。これら図面においては、操作者６０がロボット５０の先端部５８に力Ｆ３を矢印方向に作用させて、ロボット５０を移動させることを示している。このとき、ロボット５０の先端部５８は、反時計回り方向にＪ５軸５５回りに回転移動をしている。移動動作の詳細については、以下で述べる。

図２４Ａにおいては、作用される力に応じて直交座標系上の位置および／または姿勢の移動を行う第一制御モードで、ロボット５０の先端部５８を下方向に並進移動させていることが示されている。このとき、図２２Ａに示される状況とは異なり、ロボット５０の先端部５８を下方向に並進移動させるようにするため、Ｊ５軸５５のみならず、ロボット５０のＪ２軸５２、Ｊ３軸５３などの別の軸も移動している。これはロボット５０の先端部５８の姿勢を保つことに起因している。このため、図２２Ａに示される状況とは異なり、Ｊ５軸５５については、反時計回り方向にＪ５軸５５回りに移動している。このような並進移動により、図２４Ｂに示されるように、リンク８２およびリンク８３が直線をなすように互いに接近する。

本発明においては、図２４Ｂにおいて示される状態において、リンク８２およびリンク８３が直線をなすように近づいたときに、第一制御モードから第二制御モードへの切替えを行っている。その理由は、前述したのと同様である。

第二制御モードのとき、またはその直前において、操作される軸における第二力算出部２３が算出した力と、各軸の移動方向とをもとに、操作軸設定部２５は第二制御モードのときの力の方向に応じて定まる移動方向を設定する。図２４Ｂから分かるように、この場合には、Ｊ５軸５５回りの力のモーメントの向きと、Ｊ５軸５５の移動方向は逆向きである。

そして、図２４Ｃに示されるように、リンク８２およびリンク８３が直線上およびその近傍に存在するときには、第二制御モードのままである。次いで、図２４Ｄに示されるように、Ｊ５軸５５の回転移動が進んで、リンク８２およびリンク８３が直線をなさないようになると、第二制御モードから第一制御モードに切替わる。

このように図２４Ａから図２４Ｄに示される場合においては、操作者６０がロボット５０の先端部５８に力Ｆ３を作用させて移動させるときに、第一制御モードから第二制御モードに切替え、再度、第一制御モードに戻している。この動作の間、Ｊ５軸５５の回転移動方向は同じであり続けるので、第二制御モードの状態を円滑に通過することができる。なお、第一制御モードから第二制御モードに切替える、図２４Ｂにおいて、力の方向に応じて定まる移動方向として、力Ｆ３の方向とＪ５軸５５の回転移動方向とを同じ方向にして移動させるように設定した場合には、第一制御モードと第二制御モードとがその境界部分で繰返し切替わるようになり、第二制御モードで良好に移動させられない。

また、上記の動作の間、第一制御モードおよび第二制御モードにおいて、Ｊ５軸５５の回転移動方向は同じ方向であり続けるので、第二制御モードの状態を円滑に通過させて、別の第一制御モードの状態まで移動させられる。

また、図２４Ａに示す第一制御モードでの移動において、下方向に力をかけて直交座標系上の並進移動をさせるとき、ロボット５０の先端部５８が力の方向と同じ下方向に移動するので、操作者６０は、違和感を感じることはない。下方向に移動させるときに、図２４Ａの状態から、リンク８２およびリンク８３が直線をなすように近づく。このとき、力Ｆ３の方向と、Ｊ５軸５５が移動する方向とは、互いに逆になっている。

そして、図２４Ｂ、図２４Ｃに示す第二制御モードでの移動において、力Ｆ３の方向と、逆方向にＪ５軸５５が移動するようにして、第二制御モードの状態を通過させている。しかしながら、この場合には、Ｊ５軸５５だけを移動させると、力をかけた方向と逆方向にロボット５０の先端部５８が移動するので、操作者６０は、ロボット５０の先端部５８から押し返され、持ち上げられるような、違和感、操作のしにくさを感じることがある。

この違和感、操作のしにくさを軽減させるため、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を用いて、Ｊ５軸５５の原点の位置を、図２４Ｂ、図２４Ｃに示される第二制御モードでの移動において、下方向に移動させるのが好ましい。このとき、ロボット５０の先端部５８の位置が移動する方向は、Ｊ５軸５５によるロボット５０の先端部５８の移動量と、Ｊ５軸５５の原点の位置の移動量によって決まる。また、Ｊ５軸５５の原点の位置は、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３の移動によって決まる。Ｊ５軸５５の回転移動によって、ロボット５０の先端部５８の位置が力の方向とは逆方向に移動する移動量をなるべく減らすように、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を移動させる。これにより、操作者６０がロボット５０から押し返される感じを軽減させられる。なお、本実施例に示すロボット５０では、Ｊ５軸５５の原点の位置は、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３の位置によって定まる。このため、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を、操作軸として前述したように移動させることによって、Ｊ５軸５５の原点の位置を所望の方向に移動させることが可能となっている。また、ロボット５０に示される軸構成では、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３だけを移動させるとすると、それによってＪ５軸５５の原点が移動し、ロボット５０の先端部５８も同じ方向に並進移動をする。

このように、図２４Ｂ、図２４Ｃに示される第二制御モードの状態から図２４Ｄに示される状態に移動させ、第一制御モードに切替えることが望まれる。このとき、図２５に示されるように、Ｊ１軸５１、Ｊ２軸５２、Ｊ３軸５３を操作軸として移動させることによって、Ｊ５軸５５の原点の位置、ロボット５０の先端部５８の位置を下方向に移動させる移動量が大きいと、Ｊ５軸５５につながるリンク８２の姿勢が変わる。この場合、Ｊ５軸５５は図示された移動方向に移動するものの、Ｊ５軸５５を挟んだリンク８２とリンク８３との成す角度の変化量は小さい。このため、Ｊ５軸５５を挟んだリンク８２とリンク８３との成す角度の関係が図２４Ｄに示される状態になるには時間がかかるか、そのような状態に到達しない事態も生じうる。このため、力の方向とは逆方向に移動させる操作軸以外の軸を移動させることによって、Ｊ５軸５５の原点の位置、ロボット５０の先端部５８の位置を下方向に移動させる移動量は所定の値以内に抑えることが好ましい。

なお、図２６Ａから図２６Ｄには、操作者６０がロボット５０の先端部５８に力Ｆ４を矢印方向に作用させて、第一制御モードおよび第二制御モードにおいて、Ｊ５軸５５の回転移動方向を時計回り方向にＪ５軸５５回りに同じ方向に回転移動させることが示されている。この場合、図２６Ａに示される状態では第一制御モード、図２６Ｂおよび図２６Ｃでは第二制御モード、図２６Ｄでは第一制御モードで、移動させる。このような場合にも、図２４Ａから図２４Ｄに示される場合と同様な効果が得られるのは明らかであろう。

本発明の５番目の実施形態においては、ロボット制御装置１０における、操作指令部２４は、さらに、第一制御モードと第二制御モードとの二つの制御モードにおいて、一方の制御モードから他方の制御モードに切替えるとき、全ての軸を減速停止または速度を所定閾値より小さくした後に切替えるようにすることが好ましい。

これによって、ロボット５０を構成する全ての軸を安全速度まで、減速または全ての軸を減速停止させた後に、制御モードを切替えることになる。このため、制御モードの切替えのタイミングを操作者に分かりやすくするとともに、力に応じた操作をより安全に行うことが可能となる。

本発明の６番目の実施形態においては、ロボット制御装置１０における、操作指令部２４は、さらに、第一制御モードと第二制御モードとの二つの制御モードにおいて、第一制御モードから第二制御モードに切替えるとき、第二制御モードで動作させる軸以外の軸を減速停止させることが好ましい。

これによって、制御モードを第一制御モードから第二制御モードに切替えるとき、他方では動作させない軸のみを減速停止させて、他方の制御モードで操作させる軸を継続して移動させることになる。このため、移動操作を継続的に行わせたり、安全性を保ちながら目的とする位置へ素早く移動させたりすることが可能となり、操作性を向上させることが可能となる。

本発明の７番目の実施形態においては、ロボット制御装置１０は、さらに第一制御モードと第二制御モードの内、どちらの制御モードであるかの表示出力を行うともに、第二制御モードのとき、操作軸設定部２５が設定する操作軸と、該操作軸にかかる力の方向に応じて定まる移動方向の表示出力を行う表示出力部７１を備えることが好ましい。

図２０に示されるように、本発明の他の実施形態に係るロボット制御装置１０によって制御されるロボット５０を具備するロボットシステム１１では、ロボット制御装置１０の表示出力部７１の表示出力をもとに、ロボット５０の移動操作に関する各種状態を表示出力する表示装置７０が、ロボット制御装置１０に接続されていることが好ましい。

また、各種状態の確認を行うときに、操作者６０が表示装置７０を保持しなくてよいように、表示装置７０は、ロボット５０上の適当な箇所に、例えば、図２１に示されるようにロボット５０の先端部５８や、また、ロボット５０を構成する軸間をつなぐリンク上などに、ディスプレイ、表示灯などとして取付られていてもよい。また、この表示装置７０は、入力装置を兼ねた装置であってもよく、各種設定を入力する装置、かつ、ロボット５０の移動操作、停止の操作入力を行うことができる装置であってもよい。

図２、図１８に示される表示出力部７１は、ロボット５０を力に応じて移動させる状態であるか、または、教示装置などによって移動させる状態であるか、また、ロボット５０を移動させることが可能な状態になっているか否かを表示する。ロボット５０を力に応じて移動させることが可能な状態になっているか否かの表示において、力計測部２１が正しく力を計測できる状態になっているか、また、操作者がロボット５０の先端部５８に作用させる正味の力を力計測部２１が計測するために、例えば、力センサに取付られた物体の質量や重心などの必要な情報が正しく取得されているか、また、ロボット５０の軸を移動させるアクチュエータが動作可能になっているか、などを表示するのが好ましい。

また、ロボット５０を力に応じて移動することが可能な状態のとき、また、ロボット５０を力に応じて移動させているとき、表示装置７０は制御モードが第一制御モードと第二制御モードのどちらの制御モードであるかを出力表示する。また、制御モードが、第二制御モードであるとき、操作軸設定部２５によって設定された操作軸、および、操作軸の力の方向に応じて定まる移動方向も表示する。

このとき、図２に示す、ロボット制御装置１０ａの構成例では、表示出力部７１は、さらに、特異姿勢であること、特異姿勢の分類、特異姿勢の近傍にあることの要因となる軸、前記特異姿勢の近傍を通過させる軸などを表示出力する。

また、図１８に示す、ロボット制御装置１０ｂの構成例では、表示出力部７１は、さらに、所定の軸の位置が所定の位置関係の条件を満たす状態にあること、どのような所定の位置関係の条件を満たした状態であるのか、前記状態の位置にあることの要因となる軸、前記状態の位置を通過させる軸などを表示出力する。これによって、操作性を向上させることが可能となる。

以上、本発明の種々の実施形態および変形例を説明したが、他の実施形態および変形例によっても本発明の意図される作用効果を奏することができることは当業者に自明である。特に、本発明の範囲を逸脱することなく前述した実施形態および変形例の構成要素を削除ないし置換することが可能であるし、公知の部をさらに付加することが可能である。また、本明細書において明示的または暗示的に開示される複数の実施形態の特徴を任意に組合せることによっても本発明を実施できることは当業者に自明である。すなわち、本発明の技術的思想の範囲内で考えられる他の形態についても、本発明の範囲内に含まれる。

１０ ロボット制御装置
１０ａ 特異姿勢近傍判定部を備える、ロボット制御装置
１０ｂ 軸位置状態判定部を備える、ロボット制御装置
１１ ロボットシステム
２１ 力計測部
２２ 第一力算出部
２３ 第二力算出部
２４ 操作指令部
２５ 操作軸設定部
２６ 記憶部
２７ 特異姿勢近傍判定部
２８ 軸位置状態判定部
３１ 操作軸
５０ ロボット
５１ ロボットの第一軸、Ｊ１軸
５２ ロボットの第二軸、Ｊ２軸
５３ ロボットの第三軸、Ｊ３軸
５４ ロボットの第四軸、Ｊ４軸
５５ ロボットの第五軸、Ｊ５軸
５６ ロボットの第六軸、Ｊ６軸
５７ ロボットのフランジ部
５８ ロボットの先端部
５９ ロボットの台座
６０ 操作者
７０ 表示装置
７１ 表示出力部
８２ Ｊ３軸５３およびＪ４軸５４と、Ｊ５軸５５とをつなぐリンク
８３ Ｊ５軸５５およびＪ６軸５６からフランジ部５７につながるリンク
Ａｘ 操作軸に設定した座標系のＸ軸
Ａｙ 操作軸に設定した座標系のＹ軸
Ａｚ 操作軸の回転中心線と一致する、操作軸に設定した座標系のＺ軸
Ａｚ２ Ｚ軸Ａｚと、平行な軸
Ｃ Ｘ軸Ａｘと、Ｙ軸Ａｙが成す平面であり、操作軸の回転中心線と直交する平面
Ｆ 力計測部が計測した力における、並進方向の力
Ｆｎ 平面Ｃ上で、力Ｆｐを平行移動させた力
Ｆｎ１ 平面Ｃ上で、力Ｆｐを回転移動させた力
Ｆｎ２ 平面Ｃ上で、力Ｆｐを回転移動させた力
Ｆｎ３ 平面Ｃ上で、力Ｆｐを回転移動させた力
Ｆｎｐ 平面Ｃ上で、力Ｆｐを平行移動させた力
Ｆｓ 力計測部が計測した力
Ｆｓｐ 力Ｆが作用する点を平面Ｃ上に移動するように、力Ｆを平行移動させた力
Ｆｐ 操作軸の回転中心線回りに作用する、平面Ｃ上の並進方向の力
Ｌｗ 平面Ｃ上に存在し、点Ｐ１と点Ｐ２を含む直線
Ｐ１ 操作軸３１に設定した座標系の原点
Ｐ２ 力計測点Ｐ３を平面Ｃに射影した、平面Ｃ上の点
Ｐ３ 力計測点
Ｐ４ 平面Ｃ上の点
Ｐｎ 平面Ｃ上で、位置ベクトルＰｖを伸縮調整した位置ベクトル
Ｐｖ 平面Ｃ上の位置ベクトル
Ｒ１ 回転軸
Ｒ２ 回転軸
Ｒｐ 方向、角度の範囲
Ｒｐ１ 方向、角度の範囲
Ｒｐ２ 方向、角度の範囲
Ｒｐ３ 方向、角度の範囲
＋Ｒｑ 平面Ｃ上で、直線Ｌｗと直交する正方向
−Ｒｑ 平面Ｃ上で、直線Ｌｗと直交する負方向

Claims (7)

1. 複数の軸から成るロボットに作用させた力に基づいて、前記ロボットを移動させるロボットシステムの、ロボット制御装置において、
   前記ロボットの先端部に作用させた力を計測する力計測部と、
   前記力計測部が計測した力をもとに、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方の移動操作を行う操作力を算出する第一力算出部と、
   前記力計測部が計測した力をもとに、前記ロボットの各軸の位置の移動操作を行う操作力を算出する第二力算出部と、
   前記力計測部が計測した力に基づいて前記ロボットを移動させる操作指令を出力する操作指令部と、
   前記ロボットが特異姿勢の近傍にあるか否かの判定を行う特異姿勢近傍判定部と、
   前記特異姿勢近傍判定部によって前記ロボットが特異姿勢の近傍にあると判定されるときの各軸の現在位置に応じて、前記特異姿勢の近傍にあることの要因となる軸、または、前記特異姿勢の近傍を通過させる軸、を含む一つ乃至複数の所定の軸を、力に応じて移動させる操作軸として設定し、さらに力の方向に応じて定まる前記操作軸の移動方向を設定する操作軸設定部と、
   を備え、
   前記操作指令部は、
   前記特異姿勢近傍判定部により前記ロボットが特異姿勢の近傍にないと判定されるとき、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方を、前記第一力算出部が算出した前記操作力に基づいて移動させる操作指令を出力する第一制御モードと、
   前記特異姿勢近傍判定部により前記ロボットが特異姿勢の近傍にあると判定されるとき、前記操作軸設定部によって設定される前記操作軸の位置を、前記第二力算出部が算出した前記操作力と前記操作軸設定部によって設定される前記移動方向に基づいて移動させる操作指令を出力する第二制御モードとを有する、
   ことを特徴とするロボット制御装置。
2. 複数の軸から成るロボットに作用させた力に基づいて、前記ロボットを移動させるロボットシステムの、ロボット制御装置において、
   前記ロボットの先端部に作用させた力を計測する力計測部と、
   前記力計測部が計測した力をもとに、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方の移動操作を行う操作力を算出する第一力算出部と、
   前記力計測部が計測した力をもとに、前記ロボットの各軸の位置の移動操作を行う操作力を算出する第二力算出部と、
   前記力計測部が計測した力に基づいて前記ロボットを移動させる操作指令を出力する操作指令部と、
   一つ乃至複数の所定の軸の位置が、所定の位置関係の条件を満たす状態の位置にあるか否かの判定を行う軸位置状態判定部と、
   前記軸位置状態判定部によって前記所定の軸が前記状態の位置にあると判定されるときの前記ロボットの各軸の現在位置に応じて、前記状態の位置にあることの要因となる軸、または、前記状態の位置を通過させる軸、を含む一つ乃至複数の所定の軸を、力に応じて移動させる操作軸として設定し、さらに力の方向に応じて定まる前記操作軸の移動方向を設定する操作軸設定部と、
   を備え、
   前記操作指令部は、
   前記軸位置状態判定部により前記所定の軸が前記状態の位置にないと判定されるとき、前記ロボットの先端部の位置および姿勢のうちの少なくとも一方を、前記第一力算出部が算出した前記操作力に基づいて移動させる操作指令を出力する第一制御モードと、
   前記軸位置状態判定部により前記所定の軸が前記状態の位置にあると判定されるとき、前記操作軸設定部によって設定される前記操作軸の位置を、前記第二力算出部が算出した前記操作力と前記操作軸設定部によって設定される前記移動方向に基づいて移動させる操作指令を出力する第二制御モードとを有する、
   ことを特徴とするロボット制御装置。
3. 請求項１または２に記載のロボット制御装置において、
   前記操作軸設定部は、
   前記第二制御モードになる直前または前記第二制御モードになったときの、
   前記操作軸にかかる力の方向と、前記操作軸の移動方向とをもとに、
   前記操作軸にかかる力の方向に応じて定まる移動方向を設定する、
   ことを特徴とするロボット制御装置。
4. 請求項１から３のいずれか１項に記載のロボット制御装置において、
   前記操作指令部は、
   前記第二制御モードにおいて、
   前記一つ乃至複数の操作軸の内、軸にかかる力の方向とは逆方向に移動させる操作軸の原点の位置を、前記操作軸の動作によって前記ロボットの先端部の位置が移動する方向とは逆方向の成分を含む方向に移動させる、または、前記操作軸にかかる力の方向の成分を含む方向に移動させる、
   ことを特徴とするロボット制御装置。
5. 請求項１から４のいずれか１項に記載のロボット制御装置において、
   前記操作指令部は、
   前記第一制御モードと前記第二制御モードとの二つの制御モードにおいて、一方の制御モードから他方の制御モードに切替えるとき、
   全ての軸を減速停止または全ての軸の速度を所定閾値より小さくした後に切替える、
   ことを特徴とするロボット制御装置。
6. 請求項１から５のいずれか１項に記載のロボット制御装置において、
   前記操作指令部は、
   前記第一制御モードと前記第二制御モードとの二つの制御モードにおいて、前記第一制御モードから前記第二制御モードに切替えるとき、前記第二制御モードで動作させる軸以外の軸を減速停止させる、
   ことを特徴とするロボット制御装置。
7. 請求項１から６のいずれか１項に記載のロボット制御装置において、
   前記第一制御モードと前記第二制御モードの内、どちらの制御モードであるかの表示出力を行うともに、
   前記第二制御モードのとき、前記操作軸設定部が設定する前記操作軸と、該操作軸にかかる力の方向に応じて定まる移動方向の表示出力を行う表示出力部を備える、
   ことを特徴とするロボット制御装置。

Images