JP3896024B2 - 垂直多関節型マニピュレータの制御装置 - Google Patents
垂直多関節型マニピュレータの制御装置 Download PDFInfo
- Publication number
- JP3896024B2 JP3896024B2 JP2002106384A JP2002106384A JP3896024B2 JP 3896024 B2 JP3896024 B2 JP 3896024B2 JP 2002106384 A JP2002106384 A JP 2002106384A JP 2002106384 A JP2002106384 A JP 2002106384A JP 3896024 B2 JP3896024 B2 JP 3896024B2
- Authority
- JP
- Japan
- Prior art keywords
- vector
- command
- joint
- hand
- angular velocity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Description
【０００１】
【発明の属する技術分野】
本発明は、多自由度の垂直多関節型マニピュレータを制御するための制御装置に関する。
【０００２】
【従来の技術】
多関節マニピュレータでは、特異点、特異姿勢と呼ばれる動作不能点が存在する。このような特異点や動作不能点の近傍では、手先の微少な移動に対して、極めて大きな回転量を求められる関節軸が発生する。これは、同一の手先位置姿勢を実現するために、多関節マニピュレータが取りうる複数の形態の変換点でもある。
教示再生型の制御手段であっても、あるいは操縦型の制御手段であっても、動作指令中にこのような各関節軸の回転の過大指令が発生した場合には、関節駆動装置の回転速度の上限があるため、適切な制御が実現不可能となる。特異点では、このように速度と軌跡精度の両立が困難であり、多くの場合は軌跡精度を確保するために速度を低下させるか、動作停止することが一般的であった。
【０００３】
具体的には、図７に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ７−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算する（Ｓ７−２）。そして、各関節の角速度指令値が過大であるか否かを判定し（Ｓ７−３）、過大でない場合には、マニピュレータを駆動し（Ｓ７−５）、過大である場合には、マニピュレータの動作を停止する（Ｓ７−４）。
また、通常の教示再生型ロボットの教示作業は、一種の遠隔操作状態であるが、教示作業中に特異点／特異姿勢に接近すると、各軸のモーターの回転速度限界まで運転が継続し、最大回転数、ないしは最大加速度を突破する場合には、即座に停止して、制御モード変更、すなわち各軸を直接操作するモードないしは、原点復帰モードとするなどの復旧措置を講ずる必要があった。このような手法を用いた遠隔操作では、作業が中断されるので極めて非生産的になる。
【０００４】
特開平２−２１８５６９号公報、特開平３−６６５７６号公報、特開平７−３１４３６３号公報には、特異点を通過するような軌跡が判っている場合に、近似曲線で結ぶことにより速度低下を回避する技術が提案されている。しかしながら、これらの公報では、特異点の検出と近似曲線、疑似ヤコビ行列（および逆行列）などの具体的な生成方法が示されていない。
【０００５】
特開平５−５７６４３号公報には、ツールの特性から決まる許容範囲を設定し、その範囲内で特異姿勢（特異点）を回避するように姿勢を変更する技術が提案されている。しかしながら、この公報には、具体的な変更方法が示されていない。特に、時々刻々の指令変化が考えられる操縦型において、軌跡生成手段を別途設けることは非現実的である。
【０００６】
特開平６−１７０７６５号公報、特開平１１−２３９９８８号公報には、手先の速度を特異点で一旦零にした後、特異点を通過後に、本来の速度に復帰させる技術が提案されている。しかしながら、これらの公報には、一時停止から再起動、増速に転ずる時機の具体的な判定方法が示されていない。また、指令速度零は停止を意味するので、特異点に近づくとそこで停止したままとなる可能性があるなど、特異点近傍での操作が連続する際には不都合が生じる。
【０００７】
特開平６−３１２３９２号公報には、マニピュレータの構造から特異点を事前に調べる技術が提案されている。しかしながら、特異点、特異姿勢は空間的に広がっており、従来型の制御装置、制御則からは大幅な変更が必要である。
【０００８】
特開平８−１９４５１３号公報、特開平８−２４１１０８号公報には、特異点を通過するような軌跡が判っている場合に、ツールの姿勢にも考慮して、姿勢ないしは軌跡を若干変更して、完全な特異姿勢とならない技術が提案されている。しかしながら、時々刻々の指令変化が考えられる操縦型では、将来の軌跡の予測が困難であるため現実的でない。また、適用範囲が同軸手首構造に限定されている。
【０００９】
特開平１１−２４５１８４号公報には、特異点を通過するような軌跡が判っている場合に、座標系を特異点回避用の事前に用意された別の座標系に切り替えて制御する技術、例えば、軸数を減じた構成のマニピュレータの座標系に変更する技術が提案されている。しかしながら、この技術も、操縦型では将来の軌跡の予測が困難であるため現実的でない。
【００１０】
特開平１０−１３８１８０、特開平１０−３２９０６６号公報には、６軸以上の冗長自由度を有するマニピュレータを用いた技術が提案されている。しかしながら、一般市販されている工業用マニピュレータは殆んど６軸以下であり、これを流用して経済的な遠隔操作システムを構築することは困難である。
【００１１】
【発明が解決しようとする課題】
上述したように、多関節型ロボット・マニピュレータでは、特に手先直交座標系での移動、姿勢変化から各関節の回転量への順変換、逆変換の計算が制御則の構成上重要であり、計算負荷も大きい。
しかしながら、上述した従来の技術（特開平７−３１４３６３号公報、特開平１１−２３９９８８号公報、特開平１１−２４５１８４号公報）では、マニピュレータ制御装置に使われる一般的な順変換、逆変換の改造、変更を含んでいる。このため、制御装置、制御則を新たに設計し直す必要があり、ロボット制御装置、制御ソフトウェアの新規製作には、多くのコストを必要とする。この点、特に大量生産されない操縦型マニピュレータにおいては不利である。
本発明は、上述した事情に鑑み提案されたもので、順変換、逆変換を持つ従来の教示再生型ロボットの制御手法を利用して、実時間追値制御である操縦型ロボット・マニピュレータの特異点近傍における良好な操作性を確保することを目的とする。
【００１２】
【課題を解決するための手段】
本発明に係る垂直多関節型マニピュレータの制御装置は、上述した目的を達成するため、以下の特徴点を備えている。
【００１３】
（１）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節についてだけ、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により減算された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で求められる手先速度実行ベクトルと、前記手先速度指令ベクトルの差を求めるための遅れベクトル計算手段と、
前記遅れベクトル計算手段により求められる遅れベクトルを制御開始以降の時間で積算するための遅れベクトル積算手段と、
前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から絶対値減速手段に切り替えるための切替手段と、を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。
【００１４】
（２）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節について、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始した以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段により求められる目標位置姿勢ベクトルと、
前記実現位置姿勢計算手段により求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から前記絶対値減速手段に切り替えるための切替手段と、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。
【００１５】
（３）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
マニピュレータ先端の姿勢に対して手先ツール姿勢を定義可能な場合に、前記マニピュレータ先端姿勢に応じて決定される手先ツール姿勢を示す方向ベクトルを更新計算するためのツール方向ベクトル更新手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記ツール姿勢に応じて決定されるツール方向ベクトルと、前記手先速度指令ベクトルの並進移動成分との外積ベクトルと平行な方向ベクトル、ないしは前記外積ベクトルの大きさが零の場合に、事前に設定しておいた方向ベクトルであり、前記過大指令検出手段により過大指令とならない最小の大きさの迂回ベクトルを算出し、前記手先速度指令ベクトルとの和ベクトルに基づいて回避動作を行うための迂回ベクトル計算手段と、
前記迂回ベクトル計算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、
前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始して以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段で求められる目標位置姿勢ベクトルと、前記実現位置姿勢計算手段で求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、
前記過大指令検出手段により関節角速度指令値の絶対値が関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合、および前記迂回ベクトル計算手段により迂回ベクトルが事前に設定した最大値となっても前記過大指令検出手段により過大角速度指令が発生する場合に、前記手先速度指令ベクトルを修正するための手段を、前記迂回ベクトル計算手段から絶対値減速手段に切り替えるための切替手段を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成される絶対値減速手段を備えたことを特徴とする垂直多関節型マニピュレータの制御装置。
【００１６】
（４）前記絶対値減速手段において、
前記手先速度減算手段は、事前に設定された減衰率を前記手先速度指令ベクトルに乗じて求める方式で構成され、
前記過小指令検出手段は、前記手先速度減算手段における演算結果を前記過大指令検出手段により過大指令であるか否かを検出し、過大指令ではないことが検出された場合に、当該演算結果を減算された手先速度指令ベクトルとし、過大指令であることが検出された場合に、前記減衰率を既に減算された手先速度指令ベクトルに乗ずる過程を繰り返し、該繰り返し数が事前に設定された最大減衰回数を上回っても前記関節角速度指令値が関節角速度最大値より大きい場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示することを特徴とする前記（１）〜（３）の何れかに記載の垂直多関節型マニピュレータの制御装置。
【００１７】
【発明の実施の形態】
以下、図面を参照して、本発明に係る垂直多関節型マニピュレータの制御装置の実施形態を説明する。図８は、本発明の実施形態に係る垂直多関節型マニピュレータの制御装置の構成を示すブロック図である。本発明の実施形態に係る垂直多関節型マニピュレータの制御装置は、図８に示すように、操作者２０が、手先指令生成部３０を用いて、時々刻々の操作指令を制御部１０に入力する。制御部１０では、入力された操作指令（直交空間指令ベクトル）を、一旦、直交座標空間で表現される速度ベクトル、角速度ベクトルとして認識し、移動操縦型のマニピュレータ・メカニズム４０における各関節の回転指令値（モーター回転指令）を生成するようになっている。この回転指令値は、マニピュレータ用動力盤５０に入力され、マニピュレータ・メカニズム４０を動作させる。なお、制御部１０では、各関節角駆動制御部１１および特異点回避制御部１２により、マニピュレータ・メカニズム４０における各関節の回転指令値が生成される。ここで、手先指令生成部３０は、例えばジョイスティックにより構成され、制御部１０は、例えばコンピュータにより構成される。以下、本発明の実施形態に係る垂直多関節型マニピュレータの制御装置について、具体的な実施例及び絶対値減速の具体例を用いて詳細に説明する。
【００１８】
＜絶対値減速手段の具体例１＞
図１は、絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置では、特異点近傍における過大指令を検出した場合に、元の速度・角速度ベクトルの方向を維持し、その大きさだけを実現可能な速度に低下させて、マニピュレータの動作を続行する構成となっている。具体的には、図１に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ１−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ１−２）、絶対値減速手段により絶対値の減速を行う。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ１−３）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。一方、各関節の角速度指令値が過大である場合には、手先速度減算手段により、手先速度指令部で生成した手先速度指令ベクトルに基づいて手先速度を減算する（Ｓ１−４）。続いて、過小指令検出手段により、手先速度減算値が過小であるか否かを判定し（Ｓ１−５）、過小でない場合には、逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ１−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。一方、各関節の角速度指令値が過小である場合には、マニピュレータの動作を一時停止するとともに、特異点に接近した旨を表示する（Ｓ１−７）。そして、特異点の回避が行われると、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。
【００１９】
＜絶対値減速手段の具体例２＞
図２は、絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置では、実施例１に係る垂直多関節型マニピュレータの構成を、実際の計算手順に即して実現するようにしたものである。具体的には、図２に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ２−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ２−２）、絶対値減速手段により絶対値の減速を行う。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ２−３）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。一方、各関節の角速度指令値が過大である場合には、手先速度減算出手段による手先速度の減算処理を行う。すなわち、減衰率を乗算して手先速度指令ベクトルを生成し（Ｓ２−４）、この手先速度ベクトルを逆変換部により逆変換して、各関節の角速度指令値を演算する（Ｓ２−５）。続いて、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ２−６）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。一方、各関節の角速度指令値が過大である場合には、過小指令検出手段により過小指令の検出処理を行う。すなわち、減衰率の乗算課程の繰り返し数が予め定めた最大値以上であるか否かを判定し（Ｓ２−７）、最大値以下である場合には、減衰率の乗算処理（Ｓ２−４）に戻って、以降の処理を繰り返す。一方、繰り返し数が最大値以上となった場合には、マニピュレータの動作を一時停止するとともに、特異点に接近した旨を表示する（Ｓ２−８）。そして、特異点の回避が行われると、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。
【００２０】
＜第１実施例＞
図３は、第１実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第１実施例に係る垂直多関節型マニピュレータの制御装置では、特異点近傍における全体の速度低下を避けるため、過大指令を与えられた関節軸についてのみ、指令速度でなく実現可能な最大速度で回転し、そのために生じる「指令値に対する遅れ」を積算しておき、特異点近傍から離れて関節軸回転速度に余裕が出た時点で積算された遅れを回復するようになっている。ただし、積算遅れ量が大きくなり過ぎると、マニピュレータ動作が作業で許容される誤差範囲を突破するので、最大遅れ量を事前に設定しておき、これ以上の積算遅れが発生した場合には、絶対値減速手段の具体例１において説明した減速式の回避法へ切り替える方法を含むこととする。具体的には、図３に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ３−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ３−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ３−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ３−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ３−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ３−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。また、最大遅れ量を超えていない場合には、関節速度減算手段により各関節毎の角速度指令値を減算して（Ｓ３−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ３−８）。この手先速度実行ベクトルは、手先指令生成部により生成された手先速度指令ベクトルとともに遅れベクトル計算手段に入力されて遅れベクトル値（差）が計算され（Ｓ３−９）、さらに遅れベクトル積算手段により積算遅れベクトルが計算される（Ｓ３−１０）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。
【００２１】
＜第２実施例＞
図４は、第２実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第２実施例に係る垂直多関節型マニピュレータの制御装置では、第１実施例において遅れベクトルを直接積算しているのに対して、実行ベクトルと指令ベクトルを個別に積算する構成となっており、その積算値がそれぞれ実現位置姿勢、目標位置姿勢となることにより制御の状態が判りやすいという特徴がある。具体的には、図４に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ４−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ４−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ４−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ４−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ４−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ４−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。また、最大遅れ量を超えていない場合には、関節速度減算手段により各関節毎の角速度指令値を減算して（Ｓ４−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ４−８）。この手先速度実行ベクトルが、実行ベクトル積算手段（実現位置姿勢計算手段）により積算されて実現位置姿勢が得られる（Ｓ４−９）。また、手先指令生成部により生成された手先速度指令ベクトルが、指令ベクトル積算手段（目標位置姿勢計算手段）により積算されて目標位置姿勢が得られる（Ｓ４−１０）。そして、実現位置姿勢と目標位置姿勢とが遅れベクトル計算手段に入力されて、積算遅れベクトル（差）が計算される（Ｓ４−１１）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。
【００２２】
＜第３実施例＞
図５、図６は、第３実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第３実施例に係る垂直多関節型マニピュレータの制御装置は、ノズルや溶接トーチのようにツールの姿勢が定義できる場合の制御装置である。すなわち、第１実施例では、関節角速度の過大指令に対して各関節軸毎に独立で減速していたが、第３実施例では、ツール軸方向と指令ベクトルの並進移動速度方向ベクトルに直交する迂回ベクトルを生成し、元の指令ベクトルに加えて修正された指令ベクトルとすることにより、ツールの姿勢に影響なく特異点に接近することを回避する構成となっている。ただし、積算迂回量が大きくなり過ぎると、マニピュレータ動作が作業で許容される誤差範囲を突破するので、最大迂回量を事前に設定しておき、これ以上の積算迂回が発生した場合には、絶対値減速手段の具体例１において説明した減速式の回避法へ切り替える方法を含むこととする。
【００２３】
具体的には、図５に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ５−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ５−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ５−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ５−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ５−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。また、最大遅れ量を超えていない場合には、迂回ベクトル計算手段により迂回ベクトルを計算する（Ｓ５−６）。すなわち、迂回を中止する場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。また、迂回ベクトル計算手段により各関節の角速度指令値が計算された場合には、この角速度指令値に基づいて、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。なお、この迂回ベクトル計算手段における計算（Ｓ５−６）は、後に図６を参照して詳述する。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ５−８）。この手先速度実行ベクトルが、実行ベクトル積算手段（実現位置姿勢計算手段）により積算されて実現位置姿勢ベクトルが得られる（Ｓ５−９）。また、手先指令生成部により生成された手先速度指令ベクトルが、指令ベクトル積算手段（目標位置姿勢計算手段）により積算されて目標位置姿勢が得られる（Ｓ５−１０）。そして、実現位置姿勢と目標位置姿勢とが遅れベクトル計算手段に入力されて、積算遅れベクトル（差）が計算される（Ｓ５−１１）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。さらに、実現位置姿勢はツール方向ベクトル更新手段に入力されてツール方向ベクトルが更新され（Ｓ５−１２）、迂回ベクトル計算手段における計算に使用される。
【００２４】
迂回ベクトル計算手段における、迂回ベクトルの計算は図６に示すように、各関節の角速度指令値が過大であり、かつ最大遅れ量を超えている場合に、ツール方向ベクトルと追従ベクトルの並進成分とのベクトル外積を求め（Ｓ６−１）、この外積ベクトルの絶対値が「０」であるか否かを判定する（Ｓ６−２）。ここで外積ベクトルの絶対値が「０」である場合には、事前に設定した方向ベクトルを迂回ベクトルの方向として使用し（Ｓ６−３）、外積ベクトルの絶対値が「０」でない場合には、外積ベクトルの方向を迂回ベクトルの方向として使用する。（Ｓ６−４）。この時、ベクトル外積計算で得られる方向ベクトルと、反対方向の、ないしはマイナス１を乗じた方向ベクトルの２つを迂回ベクトルの方向ベクトルとし、以下どちらかを選択する手順までの計算は２つのベクトルについて実施する物とする。
続いて、迂回ベクトルの大きさを最小値に設定する。（Ｓ６−５）
【００２５】
手先指令ベクトルに基づいて迂回ベクトルと追従ベクトルの和ベクトルを求め（Ｓ６−７）、逆変換部により逆変換して各関節の角速度指令値を演算する（Ｓ６−８）。続いて、過大指令を検出したか否かを判定する（Ｓ６−９）。なお。過大指令の検出では、２個のベクトルのうち、変換後の関節角速度が、より小さくなるベクトルを採用して判定を行う。ここで、過大指令を検出しない場合には、各関節の角速度指令値を演算し（Ｓ６−１１）、図５に示すように、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方過大指令を検出した場合には、迂回ベクトルの大きさを、事前に設定しておいた増分値で増分し（Ｓ６−１１）、迂回ベクトルが最大値であるか否かを判定する（Ｓ６−６）。迂回ベクトルが事前に設定した最大値より大きければ、図５に示すように、絶対値減速手段の具体例１で説明した絶対値減速手段により、手先指令ベクトルの絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方、迂回ベクトルが最大値に達しない場合には、和ベクトルの演算まで戻り、以降の処理を行う。図９は、第３実施例におけるマニピュレータ先端のツール方向ベクトルと、指令ベクトルの並進移動速度方向ベクトルと、迂回ベクトルの関係を示す模式図である。図９では、実際のロボット・マニピュレータでよく使われる同軸手首構造において発生しやすい特異姿勢に向かう状況を示している。図１０は、第３実施例における迂回経路の概要を示す模式図である。第３実施例では、図１０に示すように、特異姿勢状態となる場合に、ツール方向および並進方向と直交する方向に迂回ベクトルを設定し、マニピュレータ先端を迂回させることにより、特異姿勢状態を迂回することができる。
【００２６】
【発明の効果】
以上説明したように、本発明に係る垂直多関節型マニピュレータの制御装置によれば、順変換、逆変換を持つ従来の教示再生型ロボットの制御手法を利用して、実時間追値制御である操縦型ロボット・マニピュレータの特異点近傍における良好な操作性を確保することができる。
すなわち、既存のロボット制御系を有効に使うことができるため、製造コストを低減することができる。例えば、最も一般的なPUMA型と呼ばれる同軸手首構造を有する垂直６軸多関節では、手先直交軸と各関節角座標の順変換部、逆変換部は多くの研究で公知の技術となっており、ロボット・マニピュレータの製造メーカー、ユーザー共に制御則、制御装置に関する知識が深いので、本発明に係る垂直多関節型マニピュレータの制御装置を追加的に適合させることが可能である。
【００２７】
ところで、多関節マニピュレータの特異点、特異姿勢における動作は、各関節の回転速度が有限であるため、特異点に極めて近い点を通過する際には、補間された手先動作は、速度が低下せざるを得ない。これは、制御手法の問題では無く幾何学的な運動軌跡の精度と速度のバランスの問題である。しかし、多くの操縦型マニピュレータの適用作業では、運動軌跡の誤差やバラツキの許容量が、通常のマニピュレータの制御の誤差、バラツキより大きい場合もあり、このような場合には誤差の許容量の範囲内で指令値を変更して、手先速度の低下を防止した方が作業効率上有効な場合もある。
この点、本発明に係る垂直多関節型マニピュレータの制御装置によれば、特異点近傍におけるマニピュレータの挙動に対して、動作速度の安定性を優先して軌跡の変化量を大きくするか、あるいは軌跡の変化が少なく大きく減速するように構成するか、制御方法と事前設定の係数で調整することが可能である。
また、本発明に係る垂直多関節型マニピュレータの制御装置は、６軸垂直多関節型ロボット・マニピュレータ、同軸手首構造を含んで冗長自由度マニピュレータ、６軸以下の自由度のマニピュレータにも適用することが可能である。
また、本発明に係る垂直多関節型マニピュレータの制御装置は、順変換、逆変換を多用しているので計算負荷は大きいが、最近の計算機の性能向上により、安価な汎用計算機を利用しても実用的な制御周期の間にこれらのアルゴリズムを実行することが可能である。
【図面の簡単な説明】
【図１】 絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図２】 絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図３】 第１実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図４】 第２実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図５】 第３実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図６】 第３実施例に係る垂直多関節型マニピュレータの制御方法（迂回ベクトル計算処理）を示すフローチャートである。
【図７】 従来の操縦型ロボット・マニピュレータの制御方法を示すフローチャートである。
【図８】 本発明の実施形態に係る垂直多関節型マニピュレータの制御装置の構成を示すブロック図である。
【図９】 第３実施例におけるマニピュレータ先端のツール方向ベクトルと、指令ベクトルの並進移動速度方向ベクトルと、迂回ベクトルの関係を示す模式図である。
【図１０】 第３実施例における迂回経路の概要を示す模式図である。
【発明の属する技術分野】
本発明は、多自由度の垂直多関節型マニピュレータを制御するための制御装置に関する。
【０００２】
【従来の技術】
多関節マニピュレータでは、特異点、特異姿勢と呼ばれる動作不能点が存在する。このような特異点や動作不能点の近傍では、手先の微少な移動に対して、極めて大きな回転量を求められる関節軸が発生する。これは、同一の手先位置姿勢を実現するために、多関節マニピュレータが取りうる複数の形態の変換点でもある。
教示再生型の制御手段であっても、あるいは操縦型の制御手段であっても、動作指令中にこのような各関節軸の回転の過大指令が発生した場合には、関節駆動装置の回転速度の上限があるため、適切な制御が実現不可能となる。特異点では、このように速度と軌跡精度の両立が困難であり、多くの場合は軌跡精度を確保するために速度を低下させるか、動作停止することが一般的であった。
【０００３】
具体的には、図７に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ７−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算する（Ｓ７−２）。そして、各関節の角速度指令値が過大であるか否かを判定し（Ｓ７−３）、過大でない場合には、マニピュレータを駆動し（Ｓ７−５）、過大である場合には、マニピュレータの動作を停止する（Ｓ７−４）。
また、通常の教示再生型ロボットの教示作業は、一種の遠隔操作状態であるが、教示作業中に特異点／特異姿勢に接近すると、各軸のモーターの回転速度限界まで運転が継続し、最大回転数、ないしは最大加速度を突破する場合には、即座に停止して、制御モード変更、すなわち各軸を直接操作するモードないしは、原点復帰モードとするなどの復旧措置を講ずる必要があった。このような手法を用いた遠隔操作では、作業が中断されるので極めて非生産的になる。
【０００４】
特開平２−２１８５６９号公報、特開平３−６６５７６号公報、特開平７−３１４３６３号公報には、特異点を通過するような軌跡が判っている場合に、近似曲線で結ぶことにより速度低下を回避する技術が提案されている。しかしながら、これらの公報では、特異点の検出と近似曲線、疑似ヤコビ行列（および逆行列）などの具体的な生成方法が示されていない。
【０００５】
特開平５−５７６４３号公報には、ツールの特性から決まる許容範囲を設定し、その範囲内で特異姿勢（特異点）を回避するように姿勢を変更する技術が提案されている。しかしながら、この公報には、具体的な変更方法が示されていない。特に、時々刻々の指令変化が考えられる操縦型において、軌跡生成手段を別途設けることは非現実的である。
【０００６】
特開平６−１７０７６５号公報、特開平１１−２３９９８８号公報には、手先の速度を特異点で一旦零にした後、特異点を通過後に、本来の速度に復帰させる技術が提案されている。しかしながら、これらの公報には、一時停止から再起動、増速に転ずる時機の具体的な判定方法が示されていない。また、指令速度零は停止を意味するので、特異点に近づくとそこで停止したままとなる可能性があるなど、特異点近傍での操作が連続する際には不都合が生じる。
【０００７】
特開平６−３１２３９２号公報には、マニピュレータの構造から特異点を事前に調べる技術が提案されている。しかしながら、特異点、特異姿勢は空間的に広がっており、従来型の制御装置、制御則からは大幅な変更が必要である。
【０００８】
特開平８−１９４５１３号公報、特開平８−２４１１０８号公報には、特異点を通過するような軌跡が判っている場合に、ツールの姿勢にも考慮して、姿勢ないしは軌跡を若干変更して、完全な特異姿勢とならない技術が提案されている。しかしながら、時々刻々の指令変化が考えられる操縦型では、将来の軌跡の予測が困難であるため現実的でない。また、適用範囲が同軸手首構造に限定されている。
【０００９】
特開平１１−２４５１８４号公報には、特異点を通過するような軌跡が判っている場合に、座標系を特異点回避用の事前に用意された別の座標系に切り替えて制御する技術、例えば、軸数を減じた構成のマニピュレータの座標系に変更する技術が提案されている。しかしながら、この技術も、操縦型では将来の軌跡の予測が困難であるため現実的でない。
【００１０】
特開平１０−１３８１８０、特開平１０−３２９０６６号公報には、６軸以上の冗長自由度を有するマニピュレータを用いた技術が提案されている。しかしながら、一般市販されている工業用マニピュレータは殆んど６軸以下であり、これを流用して経済的な遠隔操作システムを構築することは困難である。
【００１１】
【発明が解決しようとする課題】
上述したように、多関節型ロボット・マニピュレータでは、特に手先直交座標系での移動、姿勢変化から各関節の回転量への順変換、逆変換の計算が制御則の構成上重要であり、計算負荷も大きい。
しかしながら、上述した従来の技術（特開平７−３１４３６３号公報、特開平１１−２３９９８８号公報、特開平１１−２４５１８４号公報）では、マニピュレータ制御装置に使われる一般的な順変換、逆変換の改造、変更を含んでいる。このため、制御装置、制御則を新たに設計し直す必要があり、ロボット制御装置、制御ソフトウェアの新規製作には、多くのコストを必要とする。この点、特に大量生産されない操縦型マニピュレータにおいては不利である。
本発明は、上述した事情に鑑み提案されたもので、順変換、逆変換を持つ従来の教示再生型ロボットの制御手法を利用して、実時間追値制御である操縦型ロボット・マニピュレータの特異点近傍における良好な操作性を確保することを目的とする。
【００１２】
【課題を解決するための手段】
本発明に係る垂直多関節型マニピュレータの制御装置は、上述した目的を達成するため、以下の特徴点を備えている。
【００１３】
（１）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節についてだけ、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により減算された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で求められる手先速度実行ベクトルと、前記手先速度指令ベクトルの差を求めるための遅れベクトル計算手段と、
前記遅れベクトル計算手段により求められる遅れベクトルを制御開始以降の時間で積算するための遅れベクトル積算手段と、
前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から絶対値減速手段に切り替えるための切替手段と、を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。
【００１４】
（２）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節について、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始した以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段により求められる目標位置姿勢ベクトルと、
前記実現位置姿勢計算手段により求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から前記絶対値減速手段に切り替えるための切替手段と、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。
【００１５】
（３）操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
マニピュレータ先端の姿勢に対して手先ツール姿勢を定義可能な場合に、前記マニピュレータ先端姿勢に応じて決定される手先ツール姿勢を示す方向ベクトルを更新計算するためのツール方向ベクトル更新手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記ツール姿勢に応じて決定されるツール方向ベクトルと、前記手先速度指令ベクトルの並進移動成分との外積ベクトルと平行な方向ベクトル、ないしは前記外積ベクトルの大きさが零の場合に、事前に設定しておいた方向ベクトルであり、前記過大指令検出手段により過大指令とならない最小の大きさの迂回ベクトルを算出し、前記手先速度指令ベクトルとの和ベクトルに基づいて回避動作を行うための迂回ベクトル計算手段と、
前記迂回ベクトル計算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、
前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始して以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段で求められる目標位置姿勢ベクトルと、前記実現位置姿勢計算手段で求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、
前記過大指令検出手段により関節角速度指令値の絶対値が関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合、および前記迂回ベクトル計算手段により迂回ベクトルが事前に設定した最大値となっても前記過大指令検出手段により過大角速度指令が発生する場合に、前記手先速度指令ベクトルを修正するための手段を、前記迂回ベクトル計算手段から絶対値減速手段に切り替えるための切替手段を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成される絶対値減速手段を備えたことを特徴とする垂直多関節型マニピュレータの制御装置。
【００１６】
（４）前記絶対値減速手段において、
前記手先速度減算手段は、事前に設定された減衰率を前記手先速度指令ベクトルに乗じて求める方式で構成され、
前記過小指令検出手段は、前記手先速度減算手段における演算結果を前記過大指令検出手段により過大指令であるか否かを検出し、過大指令ではないことが検出された場合に、当該演算結果を減算された手先速度指令ベクトルとし、過大指令であることが検出された場合に、前記減衰率を既に減算された手先速度指令ベクトルに乗ずる過程を繰り返し、該繰り返し数が事前に設定された最大減衰回数を上回っても前記関節角速度指令値が関節角速度最大値より大きい場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示することを特徴とする前記（１）〜（３）の何れかに記載の垂直多関節型マニピュレータの制御装置。
【００１７】
【発明の実施の形態】
以下、図面を参照して、本発明に係る垂直多関節型マニピュレータの制御装置の実施形態を説明する。図８は、本発明の実施形態に係る垂直多関節型マニピュレータの制御装置の構成を示すブロック図である。本発明の実施形態に係る垂直多関節型マニピュレータの制御装置は、図８に示すように、操作者２０が、手先指令生成部３０を用いて、時々刻々の操作指令を制御部１０に入力する。制御部１０では、入力された操作指令（直交空間指令ベクトル）を、一旦、直交座標空間で表現される速度ベクトル、角速度ベクトルとして認識し、移動操縦型のマニピュレータ・メカニズム４０における各関節の回転指令値（モーター回転指令）を生成するようになっている。この回転指令値は、マニピュレータ用動力盤５０に入力され、マニピュレータ・メカニズム４０を動作させる。なお、制御部１０では、各関節角駆動制御部１１および特異点回避制御部１２により、マニピュレータ・メカニズム４０における各関節の回転指令値が生成される。ここで、手先指令生成部３０は、例えばジョイスティックにより構成され、制御部１０は、例えばコンピュータにより構成される。以下、本発明の実施形態に係る垂直多関節型マニピュレータの制御装置について、具体的な実施例及び絶対値減速の具体例を用いて詳細に説明する。
【００１８】
＜絶対値減速手段の具体例１＞
図１は、絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置では、特異点近傍における過大指令を検出した場合に、元の速度・角速度ベクトルの方向を維持し、その大きさだけを実現可能な速度に低下させて、マニピュレータの動作を続行する構成となっている。具体的には、図１に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ１−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ１−２）、絶対値減速手段により絶対値の減速を行う。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ１−３）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。一方、各関節の角速度指令値が過大である場合には、手先速度減算手段により、手先速度指令部で生成した手先速度指令ベクトルに基づいて手先速度を減算する（Ｓ１−４）。続いて、過小指令検出手段により、手先速度減算値が過小であるか否かを判定し（Ｓ１−５）、過小でない場合には、逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ１−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。一方、各関節の角速度指令値が過小である場合には、マニピュレータの動作を一時停止するとともに、特異点に接近した旨を表示する（Ｓ１−７）。そして、特異点の回避が行われると、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ１−８）。
【００１９】
＜絶対値減速手段の具体例２＞
図２は、絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置では、実施例１に係る垂直多関節型マニピュレータの構成を、実際の計算手順に即して実現するようにしたものである。具体的には、図２に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ２−１）、この手先速度指令ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ２−２）、絶対値減速手段により絶対値の減速を行う。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ２−３）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。一方、各関節の角速度指令値が過大である場合には、手先速度減算出手段による手先速度の減算処理を行う。すなわち、減衰率を乗算して手先速度指令ベクトルを生成し（Ｓ２−４）、この手先速度ベクトルを逆変換部により逆変換して、各関節の角速度指令値を演算する（Ｓ２−５）。続いて、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ２−６）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。一方、各関節の角速度指令値が過大である場合には、過小指令検出手段により過小指令の検出処理を行う。すなわち、減衰率の乗算課程の繰り返し数が予め定めた最大値以上であるか否かを判定し（Ｓ２−７）、最大値以下である場合には、減衰率の乗算処理（Ｓ２−４）に戻って、以降の処理を繰り返す。一方、繰り返し数が最大値以上となった場合には、マニピュレータの動作を一時停止するとともに、特異点に接近した旨を表示する（Ｓ２−８）。そして、特異点の回避が行われると、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ２−９）。
【００２０】
＜第１実施例＞
図３は、第１実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第１実施例に係る垂直多関節型マニピュレータの制御装置では、特異点近傍における全体の速度低下を避けるため、過大指令を与えられた関節軸についてのみ、指令速度でなく実現可能な最大速度で回転し、そのために生じる「指令値に対する遅れ」を積算しておき、特異点近傍から離れて関節軸回転速度に余裕が出た時点で積算された遅れを回復するようになっている。ただし、積算遅れ量が大きくなり過ぎると、マニピュレータ動作が作業で許容される誤差範囲を突破するので、最大遅れ量を事前に設定しておき、これ以上の積算遅れが発生した場合には、絶対値減速手段の具体例１において説明した減速式の回避法へ切り替える方法を含むこととする。具体的には、図３に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ３−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ３−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ３−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ３−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ３−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ３−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。また、最大遅れ量を超えていない場合には、関節速度減算手段により各関節毎の角速度指令値を減算して（Ｓ３−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ３−１１）。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ３−８）。この手先速度実行ベクトルは、手先指令生成部により生成された手先速度指令ベクトルとともに遅れベクトル計算手段に入力されて遅れベクトル値（差）が計算され（Ｓ３−９）、さらに遅れベクトル積算手段により積算遅れベクトルが計算される（Ｓ３−１０）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。
【００２１】
＜第２実施例＞
図４は、第２実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第２実施例に係る垂直多関節型マニピュレータの制御装置では、第１実施例において遅れベクトルを直接積算しているのに対して、実行ベクトルと指令ベクトルを個別に積算する構成となっており、その積算値がそれぞれ実現位置姿勢、目標位置姿勢となることにより制御の状態が判りやすいという特徴がある。具体的には、図４に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ４−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ４−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ４−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ４−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ４−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ４−６）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。また、最大遅れ量を超えていない場合には、関節速度減算手段により各関節毎の角速度指令値を減算して（Ｓ４−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ４−１２）。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ４−８）。この手先速度実行ベクトルが、実行ベクトル積算手段（実現位置姿勢計算手段）により積算されて実現位置姿勢が得られる（Ｓ４−９）。また、手先指令生成部により生成された手先速度指令ベクトルが、指令ベクトル積算手段（目標位置姿勢計算手段）により積算されて目標位置姿勢が得られる（Ｓ４−１０）。そして、実現位置姿勢と目標位置姿勢とが遅れベクトル計算手段に入力されて、積算遅れベクトル（差）が計算される（Ｓ４−１１）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。
【００２２】
＜第３実施例＞
図５、図６は、第３実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。第３実施例に係る垂直多関節型マニピュレータの制御装置は、ノズルや溶接トーチのようにツールの姿勢が定義できる場合の制御装置である。すなわち、第１実施例では、関節角速度の過大指令に対して各関節軸毎に独立で減速していたが、第３実施例では、ツール軸方向と指令ベクトルの並進移動速度方向ベクトルに直交する迂回ベクトルを生成し、元の指令ベクトルに加えて修正された指令ベクトルとすることにより、ツールの姿勢に影響なく特異点に接近することを回避する構成となっている。ただし、積算迂回量が大きくなり過ぎると、マニピュレータ動作が作業で許容される誤差範囲を突破するので、最大迂回量を事前に設定しておき、これ以上の積算迂回が発生した場合には、絶対値減速手段の具体例１において説明した減速式の回避法へ切り替える方法を含むこととする。
【００２３】
具体的には、図５に示すように、手先指令生成部により手先速度指令ベクトルを生成し（Ｓ５−１）、この手先速度指令ベクトルに基づいて追従ベクトル算出手段により追従ベクトルを算出する。すなわち、手先速度指令ベクトルに対して遅れベクトル積算手段により演算した積算遅れベクトル（マイナス値）を加算する（Ｓ５−２）。続いて、算出した追従ベクトルを逆変換部により逆変換して各関節の角速度指令値を演算し（Ｓ５−３）、切替手段により手先速度指令ベクトルの修正を行うための手段を切り替える。すなわち、過大指令検出手段により各関節の角速度指令値が過大であるか否かを判定し（Ｓ５−４）、過大でない場合には、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方、各関節の角速度指令値が過大である場合には、最大遅れ量を超えているか否かを判定し（Ｓ５−５）、最大遅れ量を超えている場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。また、最大遅れ量を超えていない場合には、迂回ベクトル計算手段により迂回ベクトルを計算する（Ｓ５−６）。すなわち、迂回を中止する場合には、絶対値減速手段の具体例１で説明した絶対値減速手段により絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。また、迂回ベクトル計算手段により各関節の角速度指令値が計算された場合には、この角速度指令値に基づいて、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。なお、この迂回ベクトル計算手段における計算（Ｓ５−６）は、後に図６を参照して詳述する。また、絶対値減速手段からの各関節の角速度指令値は、順変換部により順変換されて手先速度実行ベクトルが演算される（Ｓ５−８）。この手先速度実行ベクトルが、実行ベクトル積算手段（実現位置姿勢計算手段）により積算されて実現位置姿勢ベクトルが得られる（Ｓ５−９）。また、手先指令生成部により生成された手先速度指令ベクトルが、指令ベクトル積算手段（目標位置姿勢計算手段）により積算されて目標位置姿勢が得られる（Ｓ５−１０）。そして、実現位置姿勢と目標位置姿勢とが遅れベクトル計算手段に入力されて、積算遅れベクトル（差）が計算される（Ｓ５−１１）。この積算遅れベクトルは、先に説明したように、追従ベクトル算出手段に入力され追従ベクトルの算出に使用される。さらに、実現位置姿勢はツール方向ベクトル更新手段に入力されてツール方向ベクトルが更新され（Ｓ５−１２）、迂回ベクトル計算手段における計算に使用される。
【００２４】
迂回ベクトル計算手段における、迂回ベクトルの計算は図６に示すように、各関節の角速度指令値が過大であり、かつ最大遅れ量を超えている場合に、ツール方向ベクトルと追従ベクトルの並進成分とのベクトル外積を求め（Ｓ６−１）、この外積ベクトルの絶対値が「０」であるか否かを判定する（Ｓ６−２）。ここで外積ベクトルの絶対値が「０」である場合には、事前に設定した方向ベクトルを迂回ベクトルの方向として使用し（Ｓ６−３）、外積ベクトルの絶対値が「０」でない場合には、外積ベクトルの方向を迂回ベクトルの方向として使用する。（Ｓ６−４）。この時、ベクトル外積計算で得られる方向ベクトルと、反対方向の、ないしはマイナス１を乗じた方向ベクトルの２つを迂回ベクトルの方向ベクトルとし、以下どちらかを選択する手順までの計算は２つのベクトルについて実施する物とする。
続いて、迂回ベクトルの大きさを最小値に設定する。（Ｓ６−５）
【００２５】
手先指令ベクトルに基づいて迂回ベクトルと追従ベクトルの和ベクトルを求め（Ｓ６−７）、逆変換部により逆変換して各関節の角速度指令値を演算する（Ｓ６−８）。続いて、過大指令を検出したか否かを判定する（Ｓ６−９）。なお。過大指令の検出では、２個のベクトルのうち、変換後の関節角速度が、より小さくなるベクトルを採用して判定を行う。ここで、過大指令を検出しない場合には、各関節の角速度指令値を演算し（Ｓ６−１１）、図５に示すように、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方過大指令を検出した場合には、迂回ベクトルの大きさを、事前に設定しておいた増分値で増分し（Ｓ６−１１）、迂回ベクトルが最大値であるか否かを判定する（Ｓ６−６）。迂回ベクトルが事前に設定した最大値より大きければ、図５に示すように、絶対値減速手段の具体例１で説明した絶対値減速手段により、手先指令ベクトルの絶対値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動部によりマニピュレータを駆動する（Ｓ５−１３）。一方、迂回ベクトルが最大値に達しない場合には、和ベクトルの演算まで戻り、以降の処理を行う。図９は、第３実施例におけるマニピュレータ先端のツール方向ベクトルと、指令ベクトルの並進移動速度方向ベクトルと、迂回ベクトルの関係を示す模式図である。図９では、実際のロボット・マニピュレータでよく使われる同軸手首構造において発生しやすい特異姿勢に向かう状況を示している。図１０は、第３実施例における迂回経路の概要を示す模式図である。第３実施例では、図１０に示すように、特異姿勢状態となる場合に、ツール方向および並進方向と直交する方向に迂回ベクトルを設定し、マニピュレータ先端を迂回させることにより、特異姿勢状態を迂回することができる。
【００２６】
【発明の効果】
以上説明したように、本発明に係る垂直多関節型マニピュレータの制御装置によれば、順変換、逆変換を持つ従来の教示再生型ロボットの制御手法を利用して、実時間追値制御である操縦型ロボット・マニピュレータの特異点近傍における良好な操作性を確保することができる。
すなわち、既存のロボット制御系を有効に使うことができるため、製造コストを低減することができる。例えば、最も一般的なPUMA型と呼ばれる同軸手首構造を有する垂直６軸多関節では、手先直交軸と各関節角座標の順変換部、逆変換部は多くの研究で公知の技術となっており、ロボット・マニピュレータの製造メーカー、ユーザー共に制御則、制御装置に関する知識が深いので、本発明に係る垂直多関節型マニピュレータの制御装置を追加的に適合させることが可能である。
【００２７】
ところで、多関節マニピュレータの特異点、特異姿勢における動作は、各関節の回転速度が有限であるため、特異点に極めて近い点を通過する際には、補間された手先動作は、速度が低下せざるを得ない。これは、制御手法の問題では無く幾何学的な運動軌跡の精度と速度のバランスの問題である。しかし、多くの操縦型マニピュレータの適用作業では、運動軌跡の誤差やバラツキの許容量が、通常のマニピュレータの制御の誤差、バラツキより大きい場合もあり、このような場合には誤差の許容量の範囲内で指令値を変更して、手先速度の低下を防止した方が作業効率上有効な場合もある。
この点、本発明に係る垂直多関節型マニピュレータの制御装置によれば、特異点近傍におけるマニピュレータの挙動に対して、動作速度の安定性を優先して軌跡の変化量を大きくするか、あるいは軌跡の変化が少なく大きく減速するように構成するか、制御方法と事前設定の係数で調整することが可能である。
また、本発明に係る垂直多関節型マニピュレータの制御装置は、６軸垂直多関節型ロボット・マニピュレータ、同軸手首構造を含んで冗長自由度マニピュレータ、６軸以下の自由度のマニピュレータにも適用することが可能である。
また、本発明に係る垂直多関節型マニピュレータの制御装置は、順変換、逆変換を多用しているので計算負荷は大きいが、最近の計算機の性能向上により、安価な汎用計算機を利用しても実用的な制御周期の間にこれらのアルゴリズムを実行することが可能である。
【図面の簡単な説明】
【図１】 絶対値減速手段の具体例１に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図２】 絶対値減速手段の具体例２に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図３】 第１実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図４】 第２実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図５】 第３実施例に係る垂直多関節型マニピュレータの制御装置における制御方法を示すフローチャートである。
【図６】 第３実施例に係る垂直多関節型マニピュレータの制御方法（迂回ベクトル計算処理）を示すフローチャートである。
【図７】 従来の操縦型ロボット・マニピュレータの制御方法を示すフローチャートである。
【図８】 本発明の実施形態に係る垂直多関節型マニピュレータの制御装置の構成を示すブロック図である。
【図９】 第３実施例におけるマニピュレータ先端のツール方向ベクトルと、指令ベクトルの並進移動速度方向ベクトルと、迂回ベクトルの関係を示す模式図である。
【図１０】 第３実施例における迂回経路の概要を示す模式図である。
Claims (4)
- 操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節についてだけ、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により減算された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で求められる手先速度実行ベクトルと、前記手先速度指令ベクトルの差を求めるための遅れベクトル計算手段と、
前記遅れベクトル計算手段により求められる遅れベクトルを制御開始以降の時間で積算するための遅れベクトル積算手段と、
前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から絶対値減速手段に切り替えるための切替手段と、を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。 - 操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と
前記過大指令検出手段により過大指令が検出された場合に、前記関節角速度最大値を超える関節について、前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないように減算するための関節速度減算手段と、
前記関節速度減算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を 開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始した以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段により求められる目標位置姿勢ベクトルと、
前記実現位置姿勢計算手段により求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、前記過大指令検出手段により前記関節角速度指令値の絶対値が前記関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合に、前記手先速度指令ベクトルの修正を行うための手段を、前記関節速度減算手段から前記絶対値減速手段に切り替えるための切替手段と、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成されることを特徴とする垂直多関節型マニピュレータの制御装置。 - 操作者の操作入力に基づいて、マニピュレータ先端の位置および姿勢に対する直交座標系における速度および角速度指令である手先速度指令ベクトルを連続的に生成するための手先指令生成部と、前記手先指令生成部により生成された前記手先速度指令ベクトルを前記マニピュレータの各関節の関節角速度指令値に変換するための逆変換部と、前記関節角速度指令値に基づいて前記マニピュレータの関節を駆動するためのマニピュレータ関節駆動部と、各関節の関節角速度値に基づいて手先速度ベクトルを求めるための順変換部とを有する多自由度の垂直多関節型マニピュレータの制御装置において、
各関節に対する前記関節角速度指令の絶対値が、各関節毎に事前に設定した関節角速度最大値を超えることを検出するための過大指令検出手段と、
マニピュレータ先端の姿勢に対して手先ツール姿勢を定義可能な場合に、前記マニピュレータ先端姿勢に応じて決定される手先ツール姿勢を示す方向ベクトルを更新計算するためのツール方向ベクトル更新手段と、
前記過大指令検出手段により過大指令が検出された場合に、前記ツール姿勢に応じて決定されるツール方向ベクトルと、前記手先速度指令ベクトルの並進移動成分との外積ベクトルと平行な方向ベクトル、ないしは前記外積ベクトルの大きさが零の場合に、事前に設定しておいた方向ベクトルであり、前記過大指令検出手段により過大指令とならない最小の大きさの迂回ベクトルを算出し、前記手先速度指令ベクトルとの和ベクトルに基づいて回避動作を行うための迂回ベクトル計算手段と、
前記迂回ベクトル計算手段により更新された関節角速度を含めた全ての関節角速度に基づいて前記順変換部で手先速度実行ベクトルを求め、この手先速度実行ベクトルの制御動作を開始した以降、積分して得られる現在の実現されている位置姿勢を計算するための実現位置姿勢計算手段と、
前記手先指令生成部により生成される前記手先速度指令ベクトルの制御動作を開始して以降、積分して得られる現在の目標位置姿勢を計算するための目標位置姿勢計算手段と、
前記目標位置姿勢計算手段で求められる目標位置姿勢ベクトルと、前記実現位置姿勢計算手段で求められる実現位置姿勢ベクトルの差を求めるための積算遅れベクトル計算手段と、
前記過大指令検出手段により関節角速度指令値の絶対値が関節角速度最大値を超えないことが検出された場合に、前記積算遅れベクトルと反対方向の復帰ベクトルを求めるとともに、前記手先速度指令ベクトルと前記復帰ベクトルとの和ベクトルを求めるための追従ベクトル算出手段と、
前記遅れベクトル積算手段により積算される積算遅れベクトルの絶対値が、事前に設定した最大遅れ量を超えることが検出され、かつ前記過大指令検出手段により過大指令が検出された場合、および前記迂回ベクトル計算手段により迂回ベクトルが事前に設定した最大値となっても前記過大指令検出手段により過大角速度指令が発生する場合に、前記手先速度指令ベクトルを修正するための手段を、前記迂回ベクトル計算手段から絶対値減速手段に切り替えるための切替手段を有し、
当該絶対値減速手段は、前記手先速度指令ベクトルの絶対値が前記関節角速度最大値を超えないように減算して、前記手先速度指令ベクトルを変更するための手先速度減算手段と、
前記手先速度減算手段により減算された手先速度指令ベクトルに基づいて、前記逆変換部で変換された前記関節角速度指令値の絶対値が、事前に設定した関節角速度最小値を下回ることが検出された場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示するための過小指令検出手段と、を含んで構成される絶対値減速手段を備えたことを特徴とする垂直多関節型マニピュレータの制御装置。 - 前記絶対値減速手段において、
前記手先速度減算手段は、事前に設定された減衰率を前記手先速度指令ベクトルに乗じて求める方式で構成され、
前記過小指令検出手段は、前記手先速度減算手段における演算結果を前記過大指令検出手段により過大指令であるか否かを検出し、過大指令ではないことが検出された場合に、当該演算結果を減算された手先速度指令ベクトルとし、過大指令であることが検出された場合に、前記減衰率を既に減算された手先速度指令ベクトルに乗ずる過程を繰り返し、該繰り返し数が事前に設定された最大減衰回数を上回っても前記関節角速度指令値が関節角速度最大値より大きい場合に、前記マニピュレータを一時停止させるとともに、操作者に対して特異点接近を表示することを特徴とする請求項１〜３の何れかに記載の垂直多関節型マニピュレータの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002106384A JP3896024B2 (ja) | 2002-04-09 | 2002-04-09 | 垂直多関節型マニピュレータの制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002106384A JP3896024B2 (ja) | 2002-04-09 | 2002-04-09 | 垂直多関節型マニピュレータの制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003300183A JP2003300183A (ja) | 2003-10-21 |
| JP3896024B2 true JP3896024B2 (ja) | 2007-03-22 |
Family
ID=29390721
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002106384A Expired - Fee Related JP3896024B2 (ja) | 2002-04-09 | 2002-04-09 | 垂直多関節型マニピュレータの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3896024B2 (ja) |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB0508217D0 (en) * | 2005-04-25 | 2005-06-01 | Renishaw Plc | Method for scanning the surface of a workpiece |
| GB0707720D0 (en) | 2007-04-23 | 2007-05-30 | Renishaw Plc | Apparatus and method for controlling or programming a measurement routine |
| GB0716218D0 (en) | 2007-08-20 | 2007-09-26 | Renishaw Plc | Measurement path generation |
| JP5726052B2 (ja) | 2011-03-08 | 2015-05-27 | 株式会社神戸製鋼所 | 多関節型ロボットの制御装置，制御方法及び制御プログラム |
| JP6238628B2 (ja) | 2013-08-06 | 2017-11-29 | キヤノン株式会社 | ロボット装置、ロボット制御方法、ロボット制御プログラム及びロボット装置を用いた部品の製造方法 |
| CN105437235A (zh) * | 2016-01-25 | 2016-03-30 | 珠海格力电器股份有限公司 | 一种奇异点区域减速保护方法、系统和工业机器人 |
| JP6429977B2 (ja) * | 2017-10-31 | 2018-11-28 | キヤノン株式会社 | ロボット装置及びロボット制御方法 |
| CN109605369B (zh) * | 2018-12-07 | 2021-09-07 | 英华达（上海）科技有限公司 | 机械臂奇异点控制方法及系统 |
| CN114378830B (zh) * | 2022-02-18 | 2024-02-20 | 深圳市大族机器人有限公司 | 一种机器人腕关节奇异规避方法及系统 |
-
2002
- 2002-04-09 JP JP2002106384A patent/JP3896024B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003300183A (ja) | 2003-10-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3896024B2 (ja) | 垂直多関節型マニピュレータの制御装置 | |
| JP5144035B2 (ja) | 産業用ロボットのツールの位置・姿勢制御方法及び制御システム | |
| JP2011206886A (ja) | ロボットの制御装置及び方法 | |
| Murakami et al. | Robust motion control based on projection plane in redundant manipulator | |
| Adorno et al. | Towards a cooperative framework for interactive manipulation involving a human and a humanoid | |
| Williams et al. | Kinesthetic force/moment feedback via active exoskeleton | |
| Hayakawa et al. | Singularity avoidance by inputting angular velocity to a redundant axis during cooperative control of a teleoperated dual-arm robot | |
| JP3206765B2 (ja) | マスタスレーブマニピュレータの制御方法 | |
| JP3748454B2 (ja) | 産業用ロボットの制御装置 | |
| Matsumoto et al. | Operation assist for a teleoperated robot system controlled with a lightweight and high-operable master device | |
| JPH08229858A (ja) | マスタスレーブマニピュレータの制御方法およびその装置 | |
| Dong et al. | Bimanual Continuous Steering Wheel Turning by a Dual-Arm Robot | |
| JP3063863B2 (ja) | マニピュレ−タの操作装置 | |
| Fukushima et al. | Online compensation of gravity and friction for haptics with incremental position sensors | |
| JPH07314363A (ja) | 多関節マニピュレータの運動制御方法及び装置 | |
| Sinaga et al. | INVERS KINEMATIK ROBOT ARM 4 DOF MENGUNAKAN SENSOR LEAP MOTION | |
| JP2001100828A (ja) | ロボット制御装置 | |
| JPH058187A (ja) | ロボツト | |
| JP2649463B2 (ja) | 回転外部軸を有する産業用ロボットの教示方法およびそれに用いる教示装置 | |
| Boshlyakov et al. | Designing a force feedback operator console for manipulator grip control | |
| JPS63289606A (ja) | 冗長自由度形ロボット | |
| JP3864237B2 (ja) | ロボットの制御方法および制御装置 | |
| JPH0631664A (ja) | 倣い制御ロボットの制御装置 | |
| Lim et al. | A real-time control system for a mobile dexterous 7 dof arm | |
| JP3930956B2 (ja) | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040901 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20060814 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20060823 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20061020 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20061206 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20061215 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 3896024 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101222 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101222 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111222 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111222 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121222 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121222 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131222 Year of fee payment: 7 |
|
| S531 | Written request for registration of change of domicile |
Free format text: JAPANESE INTERMEDIATE CODE: R313531 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131222 Year of fee payment: 7 |
|
| S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131222 Year of fee payment: 7 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| LAPS | Cancellation because of no payment of annual fees |