JP2003300183A - 垂直多関節型マニピュレータの制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 順変換、逆変換を持つ従来の教示再生型ロボ
ットの制御手法を利用して、実時間追値制御である操縦
型ロボット・マニピュレータの特異点近傍における良好
な操作性を確保する。 【解決手段】 特異点近傍における過大指令を検出した
場合（Ｓ１−３）に、絶対値減速手段により、元の速度
・角速度ベクトルの方向を維持し、その大きさだけを実
現可能な速度に低下させて（Ｓ１−４〜Ｓ１−８）、ま
たは、一部の関節の回転指令を遅らせることで（Ｓ３−
１〜Ｓ３−１１）、または、迂回ベクトルを生成して加
えることで（Ｓ５−１〜Ｓ５−１３）マニピュレータの
動作を続行する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、多自由度の垂直多
関節型マニピュレータを制御するための制御装置に関す
る。

【０００２】

【従来の技術】多関節マニピュレータでは、特異点、特
異姿勢と呼ばれる動作不能点が存在する。このような特
異点や動作不能点の近傍では、手先の微少な移動に対し
て、極めて大きな回転量を求められる関節軸が発生す
る。これは、同一の手先位置姿勢を実現するために、多
関節マニピュレータが取りうる複数の形態の変換点でも
ある。教示再生型の制御手段であっても、あるいは操縦
型の制御手段であっても、動作指令中にこのような各関
節軸の回転の過大指令が発生した場合には、関節駆動装
置の回転速度の上限があるため、適切な制御が実現不可
能となる。特異点では、このように速度と軌跡精度の両
立が困難であり、多くの場合は軌跡精度を確保するため
に速度を低下させるか、動作停止することが一般的であ
った。

【０００３】具体的には、図７に示すように、手先指令
生成部により手先速度指令ベクトルを生成し（Ｓ７−
１）、この手先速度指令ベクトルを逆変換部により逆変
換して各関節の角速度指令値を演算する（Ｓ７−２）。
そして、各関節の角速度指令値が過大であるか否かを判
定し（Ｓ７−３）、過大でない場合には、マニピュレー
タを駆動し（Ｓ７−５）、過大である場合には、マニピ
ュレータの動作を停止する（Ｓ７−４）。また、通常の
教示再生型ロボットの教示作業は、一種の遠隔操作状態
であるが、教示作業中に特異点／特異姿勢に接近する
と、各軸のモーターの回転速度限界まで運転が継続し、
最大回転数、ないしは最大加速度を突破する場合には、
即座に停止して、制御モード変更、すなわち各軸を直接
操作するモードないしは、原点復帰モードとするなどの
復旧措置を講ずる必要があった。このような手法を用い
た遠隔操作では、作業が中断されるので極めて非生産的
になる。

【０００４】特開平２−２１８５６９号公報、特開平３
−６６５７６号公報、特開平７−３１４３６３号公報に
は、特異点を通過するような軌跡が判っている場合に、
近似曲線で結ぶことにより速度低下を回避する技術が提
案されている。しかしながら、これらの公報では、特異
点の検出と近似曲線、疑似ヤコビ行列（および逆行列）
などの具体的な生成方法が示されていない。

【０００５】特開平５−５７６４３号公報には、ツール
の特性から決まる許容範囲を設定し、その範囲内で特異
姿勢（特異点）を回避するように姿勢を変更する技術が
提案されている。しかしながら、この公報には、具体的
な変更方法が示されていない。特に、時々刻々の指令変
化が考えられる操縦型において、軌跡生成手段を別途設
けることは非現実的である。

【０００６】特開平６−１７０７６５号公報、特開平１
１−２３９９８８号公報には、手先の速度を特異点で一
旦零にした後、特異点を通過後に、本来の速度に復帰さ
せる技術が提案されている。しかしながら、これらの公
報には、一時停止から再起動、増速に転ずる時機の具体
的な判定方法が示されていない。また、指令速度零は停
止を意味するので、特異点に近づくとそこで停止したま
まとなる可能性があるなど、特異点近傍での操作が連続
する際には不都合が生じる。

【０００７】特開平６−３１２３９２号公報には、マニ
ピュレータの構造から特異点を事前に調べる技術が提案
されている。しかしながら、特異点、特異姿勢は空間的
に広がっており、従来型の制御装置、制御則からは大幅
な変更が必要である。

【０００８】特開平８−１９４５１３号公報、特開平８
−２４１１０８号公報には、特異点を通過するような軌
跡が判っている場合に、ツールの姿勢にも考慮して、姿
勢ないしは軌跡を若干変更して、完全な特異姿勢となら
ない技術が提案されている。しかしながら、時々刻々の
指令変化が考えられる操縦型では、将来の軌跡の予測が
困難であるため現実的でない。また、適用範囲が同軸手
首構造に限定されている。

【０００９】特開平１１−２４５１８４号公報には、特
異点を通過するような軌跡が判っている場合に、座標系
を特異点回避用の事前に用意された別の座標系に切り替
えて制御する技術、例えば、軸数を減じた構成のマニピ
ュレータの座標系に変更する技術が提案されている。し
かしながら、この技術も、操縦型では将来の軌跡の予測
が困難であるため現実的でない。

【００１０】特開平１０−１３８１８０、特開平１０−
３２９０６６号公報には、６軸以上の冗長自由度を有す
るマニピュレータを用いた技術が提案されている。しか
しながら、一般市販されている工業用マニピュレータは
殆んど６軸以下であり、これを流用して経済的な遠隔操
作システムを構築することは困難である。

【００１１】

【発明が解決しようとする課題】上述したように、多関
節型ロボット・マニピュレータでは、特に手先直交座標
系での移動、姿勢変化から各関節の回転量への順変換、
逆変換の計算が制御則の構成上重要であり、計算負荷も
大きい。しかしながら、上述した従来の技術（特開平７
−３１４３６３号公報、特開平１１−２３９９８８号公
報、特開平１１−２４５１８４号公報）では、マニピュ
レータ制御装置に使われる一般的な順変換、逆変換の改
造、変更を含んでいる。このため、制御装置、制御則を
新たに設計し直す必要があり、ロボット制御装置、制御
ソフトウェアの新規製作には、多くのコストを必要とす
る。この点、特に大量生産されない操縦型マニピュレー
タにおいては不利である。本発明は、上述した事情に鑑
み提案されたもので、順変換、逆変換を持つ従来の教示
再生型ロボットの制御手法を利用して、実時間追値制御
である操縦型ロボット・マニピュレータの特異点近傍に
おける良好な操作性を確保することを目的とする。

【００１２】

【課題を解決するための手段】本発明に係る垂直多関節
型マニピュレータの制御装置は、上述した目的を達成す
るため、以下の特徴点を備えている。すなわち、本発明
に係る垂直多関節型マニピュレータの制御装置は、操作
者の操作入力に基づいて、マニピュレータ先端の位置お
よび姿勢に対する直交座標系における速度および角速度
指令である手先速度指令ベクトルを連続的に生成するた
めの手先指令生成部と、前記手先指令生成部により生成
された前記手先速度指令ベクトルを前記マニピュレータ
の各関節の関節角速度指令値に変換するための逆変換部
と、前記関節角速度指令値に基づいて前記マニピュレー
タの関節を駆動するためのマニピュレータ関節駆動部
と、各関節の関節角速度値に基づいて手先速度ベクトル
を求めるための順変換部とを有する多自由度の垂直多関
節型マニピュレータの制御装置において、各関節に対す
る前記関節角速度指令の絶対値が、各関節毎に事前に設
定した関節角速度最大値を超えることを検出するための
過大指令検出手段と、前記過大指令検出手段により過大
指令が検出された場合に、前記手先速度指令ベクトルの
絶対値が前記関節角速度最大値を超えないように減算し
て、前記手先速度指令ベクトルを変更するための手先速
度減算手段と、前記手先速度減算手段により減算された
手先速度指令ベクトルに基づいて、前記逆変換部で変換
された前記関節角速度指令値の絶対値が、事前に設定し
た関節角速度最小値を下回ることが検出された場合に、
前記マニピュレータを一時停止させるとともに、操作者
に対して特異点接近を表示するための過小指令検出手段
とを含んで構成される絶対値減速手段を備えたことを特
徴とするものである。

【００１３】この場合、前記絶対値減速手段において、
前記手先速度減算手段は、事前に設定された減衰率を前
記手先速度指令ベクトルに乗じて求める方式で構成さ
れ、前記過小指令検出手段は、前記手先速度減算手段に
おける演算結果を前記過大指令検出手段により過大指令
であるか否かを検出し、過大指令ではないことが検出さ
れた場合に、当該演算結果を減算された手先速度指令ベ
クトルとし、過大指令であることが検出された場合に、
前記減衰率を既に減算された手先速度指令ベクトルに乗
ずる過程を繰り返し、該繰り返し数が事前に設定された
最大減衰回数を上回っても前記関節角速度指令値が関節
角速度最大値より大きい場合に、前記マニピュレータを
一時停止させるとともに、操作者に対して特異点接近を
表示するように構成することが可能である。

【００１４】また、前記垂直多関節型マニピュレータの
制御装置において、前記過大指令検出手段により過大指
令が検出された場合に、前記関節角速度最大値を超える
関節についてだけ、前記関節角速度指令値の絶対値が前
記関節角速度最大値を超えないように減算するための関
節速度減算手段と、前記関節速度減算手段により減算さ
れた関節角速度を含めた全ての関節角速度に基づいて前
記順変換部で求められる手先速度実行ベクトルと、前記
手先速度指令ベクトルの差を求めるための遅れベクトル
計算手段と、前記遅れベクトル計算手段により求められ
る遅れベクトルを制御開始以降の時間で積算するための
遅れベクトル積算手段と、前記過大指令検出手段により
前記関節角速度指令値の絶対値が前記関節角速度最大値
を超えないことが検出された場合に、前記積算遅れベク
トルと反対方向の復帰ベクトルを求めるとともに、前記
手先速度指令ベクトルと前記復帰ベクトルとの和ベクト
ルを求めるための追従ベクトル算出手段と、前記遅れベ
クトル積算手段により積算される積算遅れベクトルの絶
対値が、事前に設定した最大遅れ量を超えることが検出
され、かつ前記過大指令検出手段により過大指令が検出
された場合に、前記手先速度指令ベクトルの修正を行う
ための手段を、前記関節速度減算手段から前記絶対値減
速手段に切り替えるための切替手段とを有して構成する
ことが可能である。

【００１５】また、前記垂直多関節型マニピュレータの
制御装置において、前記過大指令検出手段により過大指
令が検出された場合に、前記関節角速度最大値を超える
関節について、前記関節角速度指令値の絶対値が前記関
節角速度最大値を超えないように減算するための関節速
度減算手段と、前記関節速度減算手段により更新された
関節角速度を含めた全ての関節角速度に基づいて前記順
変換部で手先速度実行ベクトルを求め、この手先速度実
行ベクトルの制御動作を開始した以降、積分して得られ
る現在の実現されている位置姿勢を計算するための実現
位置姿勢計算手段と、前記手先指令生成部により生成さ
れる前記手先速度指令ベクトルの制御動作を開始した以
降、積分して得られる現在の目標位置姿勢を計算するた
めの目標位置姿勢計算手段と、前記目標位置姿勢計算手
段により求められる目標位置姿勢ベクトルと、前記実現
位置姿勢計算手段により求められる実現位置姿勢ベクト
ルの差を求めるための積算遅れベクトル計算手段と、前
記過大指令検出手段により前記関節角速度指令値の絶対
値が前記関節角速度最大値を超えないことが検出された
場合に、前記積算遅れベクトルと反対方向の復帰ベクト
ルを求めるとともに、前記手先速度指令ベクトルと前記
復帰ベクトルとの和ベクトルを求めるための追従ベクト
ル算出手段と、前記遅れベクトル積算手段により積算さ
れる積算遅れベクトルの絶対値が、事前に設定した最大
遅れ量を超えることが検出され、かつ前記過大指令検出
手段により過大指令が検出された場合に、前記手先速度
指令ベクトルの修正を行うための手段を、前記関節速度
減算手段から前記絶対値減速手段に切り替えるための切
替手段とを有して構成することが可能である。

【００１６】また、前記垂直多関節型マニピュレータの
制御装置において、マニピュレータ先端の姿勢に対して
手先ツール姿勢を定義可能な場合に、前記マニピュレー
タ先端姿勢に応じて決定される手先ツール姿勢を示す方
向ベクトルを更新計算するためのツール方向ベクトル更
新手段と、前記過大指令検出手段により過大指令が検出
された場合に、前記ツール姿勢に応じて決定されるツー
ル方向ベクトルと、前記手先速度指令ベクトルの並進移
動成分との外積ベクトルと平行な方向ベクトル、ないし
は前記外積ベクトルの大きさが零の場合に、事前に設定
しておいた方向ベクトルであり、前記過大指令検出手段
により過大指令とならない最小の大きさの迂回ベクトル
を算出し、前記手先速度指令ベクトルとの和ベクトルに
基づいて回避動作を行うための迂回ベクトル計算手段
と、前記迂回ベクトル計算手段により更新された関節角
速度を含めた全ての関節角速度に基づいて前記順変換部
で手先速度実行ベクトルを求め、この手先速度実行ベク
トルの制御動作を開始した以降、積分して得られる現在
の実現されている位置姿勢を計算するための実現位置姿
勢計算手段と、前記手先指令生成部により生成される前
記手先速度指令ベクトルの制御動作を開始して以降、積
分して得られる現在の目標位置姿勢を計算するための目
標位置姿勢計算手段と、前記目標位置姿勢計算手段で求
められる目標位置姿勢ベクトルと、前記実現位置姿勢計
算手段で求められる実現位置姿勢ベクトルの差を求める
ための積算遅れベクトル計算手段と、前記過大指令検出
手段により関節角速度指令値の絶対値が関節角速度最大
値を超えないことが検出された場合に、前記積算遅れベ
クトルと反対方向の復帰ベクトルを求めるとともに、前
記手先速度指令ベクトルと前記復帰ベクトルとの和ベク
トルを求めるための追従ベクトル算出手段と、 前記遅
れベクトル積算手段により積算される積算遅れベクトル
の絶対値が、事前に設定した最大遅れ量を超えることが
検出され、かつ前記過大指令検出手段により過大指令が
検出された場合、および前記迂回ベクトル計算手段によ
り迂回ベクトルが事前に設定した最大値となっても前記
過大指令検出手段により過大角速度指令が発生する場合
に、前記手先速度指令ベクトルを修正するための手段
を、前記迂回ベクトル計算手段から前記絶対値減速手段
に切り替えるための切替手段を有して構成することが可
能である。

【００１７】

【発明の実施の形態】以下、図面を参照して、本発明に
係る垂直多関節型マニピュレータの制御装置の実施形態
を説明する。図８は、本発明の実施形態に係る垂直多関
節型マニピュレータの制御装置の構成を示すブロック図
である。本発明の実施形態に係る垂直多関節型マニピュ
レータの制御装置は、図８に示すように、操作者２０
が、手先指令生成部３０を用いて、時々刻々の操作指令
を制御部１０に入力する。制御部１０では、入力された
操作指令（直交空間指令ベクトル）を、一旦、直交座標
空間で表現される速度ベクトル、角速度ベクトルとして
認識し、移動操縦型のマニピュレータ・メカニズム４０
における各関節の回転指令値（モーター回転指令）を生
成するようになっている。この回転指令値は、マニピュ
レータ用動力盤５０に入力され、マニピュレータ・メカ
ニズム４０を動作させる。なお、制御部１０では、各関
節角駆動制御部１１および特異点回避制御部１２によ
り、マニピュレータ・メカニズム４０における各関節の
回転指令値が生成される。ここで、手先指令生成部３０
は、例えばジョイスティックにより構成され、制御部１
０は、例えばコンピュータにより構成される。以下、本
発明の実施形態に係る垂直多関節型マニピュレータの制
御装置について、具体的な実施例を用いて詳細に説明す
る。

【００１８】＜第１実施例＞図１は、第１実施例に係る
垂直多関節型マニピュレータの制御装置における制御方
法を示すフローチャートである。第１実施例に係る垂直
多関節型マニピュレータの制御装置では、特異点近傍に
おける過大指令を検出した場合に、元の速度・角速度ベ
クトルの方向を維持し、その大きさだけを実現可能な速
度に低下させて、マニピュレータの動作を続行する構成
となっている。具体的には、図１に示すように、手先指
令生成部により手先速度指令ベクトルを生成し（Ｓ１−
１）、この手先速度指令ベクトルを逆変換部により逆変
換して各関節の角速度指令値を演算し（Ｓ１−２）、絶
対値減速手段により絶対値の減速を行う。すなわち、過
大指令検出手段により各関節の角速度指令値が過大であ
るか否かを判定し（Ｓ１−３）、過大でない場合には、
マニピュレータ関節駆動部によりマニピュレータを駆動
する（Ｓ１−８）。一方、各関節の角速度指令値が過大
である場合には、手先速度減算手段により、手先速度指
令部で生成した手先速度指令ベクトルに基づいて手先速
度を減算する（Ｓ１−４）。続いて、過小指令検出手段
により、手先速度減算値が過小であるか否かを判定し
（Ｓ１−５）、過小でない場合には、逆変換部により逆
変換して各関節の角速度指令値を演算し（Ｓ１−６）、
マニピュレータ関節駆動部によりマニピュレータを駆動
する（Ｓ１−８）。一方、各関節の角速度指令値が過小
である場合には、マニピュレータの動作を一時停止する
とともに、特異点に接近した旨を表示する（Ｓ１−
７）。そして、特異点の回避が行われると、マニピュレ
ータ関節駆動部によりマニピュレータを駆動する（Ｓ１
−８）。

【００１９】＜第２実施例＞図２は、第２実施例に係る
垂直多関節型マニピュレータの制御装置における制御方
法を示すフローチャートである。第２実施例に係る垂直
多関節型マニピュレータの制御装置では、実施例１に係
る垂直多関節型マニピュレータの構成を、実際の計算手
順に即して実現するようにしたものである。具体的に
は、図２に示すように、手先指令生成部により手先速度
指令ベクトルを生成し（Ｓ２−１）、この手先速度指令
ベクトルを逆変換部により逆変換して各関節の角速度指
令値を演算し（Ｓ２−２）、絶対値減速手段により絶対
値の減速を行う。すなわち、過大指令検出手段により各
関節の角速度指令値が過大であるか否かを判定し（Ｓ２
−３）、過大でない場合には、マニピュレータ関節駆動
部によりマニピュレータを駆動する（Ｓ２−９）。一
方、各関節の角速度指令値が過大である場合には、手先
速度減算出手段による手先速度の減算処理を行う。すな
わち、減衰率を乗算して手先速度指令ベクトルを生成し
（Ｓ２−４）、この手先速度ベクトルを逆変換部により
逆変換して、各関節の角速度指令値を演算する（Ｓ２−
５）。続いて、過大指令検出手段により各関節の角速度
指令値が過大であるか否かを判定し（Ｓ２−６）、過大
でない場合には、マニピュレータ関節駆動部によりマニ
ピュレータを駆動する（Ｓ２−９）。一方、各関節の角
速度指令値が過大である場合には、過小指令検出手段に
より過小指令の検出処理を行う。すなわち、減衰率の乗
算課程の繰り返し数が予め定めた最大値以上であるか否
かを判定し（Ｓ２−７）、最大値以下である場合には、
減衰率の乗算処理（Ｓ２−４）に戻って、以降の処理を
繰り返す。一方、繰り返し数が最大値以上となった場合
には、マニピュレータの動作を一時停止するとともに、
特異点に接近した旨を表示する（Ｓ２−８）。そして、
特異点の回避が行われると、マニピュレータ関節駆動部
によりマニピュレータを駆動する（Ｓ２−９）。

【００２０】＜第３実施例＞図３は、第３実施例に係る
垂直多関節型マニピュレータの制御装置における制御方
法を示すフローチャートである。第３実施例に係る垂直
多関節型マニピュレータの制御装置では、特異点近傍に
おける全体の速度低下を避けるため、過大指令を与えら
れた関節軸についてのみ、指令速度でなく実現可能な最
大速度で回転し、そのために生じる「指令値に対する遅
れ」を積算しておき、特異点近傍から離れて関節軸回転
速度に余裕が出た時点で積算された遅れを回復するよう
になっている。ただし、積算遅れ量が大きくなり過ぎる
と、マニピュレータ動作が作業で許容される誤差範囲を
突破するので、最大遅れ量を事前に設定しておき、これ
以上の積算遅れが発生した場合には、第１実施例におい
て説明した減速式の回避法へ切り替える方法を含むこと
とする。具体的には、図３に示すように、手先指令生成
部により手先速度指令ベクトルを生成し（Ｓ３−１）、
この手先速度指令ベクトルに基づいて追従ベクトル算出
手段により追従ベクトルを算出する。すなわち、手先速
度指令ベクトルに対して遅れベクトル積算手段により演
算した積算遅れベクトル（マイナス値）を加算する（Ｓ
３−２）。続いて、算出した追従ベクトルを逆変換部に
より逆変換して各関節の角速度指令値を演算し（Ｓ３−
３）、切替手段により手先速度指令ベクトルの修正を行
うための手段を切り替える。すなわち、過大指令検出手
段により各関節の角速度指令値が過大であるか否かを判
定し（Ｓ３−４）、過大でない場合には、マニピュレー
タ関節駆動部によりマニピュレータを駆動する（Ｓ３−
１１）。一方、各関節の角速度指令値が過大である場合
には、最大遅れ量を超えているか否かを判定し（Ｓ３−
５）、最大遅れ量を超えている場合には、第１実施例で
説明した絶対値減速手段により絶対値の減速を行い（Ｓ
３−６）、マニピュレータ関節駆動部によりマニピュレ
ータを駆動する（Ｓ３−１１）。また、最大遅れ量を超
えていない場合には、関節速度減算手段により各関節毎
の角速度指令値を減算して（Ｓ３−７）、マニピュレー
タ関節駆動部によりマニピュレータを駆動する（Ｓ３−
１１）。また、絶対値減速手段からの各関節の角速度指
令値は、順変換部により順変換されて手先速度実行ベク
トルが演算される（Ｓ３−８）。この手先速度実行ベク
トルは、手先指令生成部により生成された手先速度指令
ベクトルとともに遅れベクトル計算手段に入力されて遅
れベクトル値（差）が計算され（Ｓ３−９）、さらに遅
れベクトル積算手段により積算遅れベクトルが計算され
る（Ｓ３−１０）。この積算遅れベクトルは、先に説明
したように、追従ベクトル算出手段に入力され追従ベク
トルの算出に使用される。

【００２１】＜第４実施例＞図４は、第４実施例に係る
垂直多関節型マニピュレータの制御装置における制御方
法を示すフローチャートである。第４実施例に係る垂直
多関節型マニピュレータの制御装置では、第３実施例に
おいて遅れベクトルを直接積算しているのに対して、実
行ベクトルと指令ベクトルを個別に積算する構成となっ
ており、その積算値がそれぞれ実現位置姿勢、目標位置
姿勢となることにより制御の状態が判りやすいという特
徴がある。具体的には、図４に示すように、手先指令生
成部により手先速度指令ベクトルを生成し（Ｓ４−
１）、この手先速度指令ベクトルに基づいて追従ベクト
ル算出手段により追従ベクトルを算出する。すなわち、
手先速度指令ベクトルに対して遅れベクトル積算手段に
より演算した積算遅れベクトル（マイナス値）を加算す
る（Ｓ４−２）。続いて、算出した追従ベクトルを逆変
換部により逆変換して各関節の角速度指令値を演算し
（Ｓ４−３）、切替手段により手先速度指令ベクトルの
修正を行うための手段を切り替える。すなわち、過大指
令検出手段により各関節の角速度指令値が過大であるか
否かを判定し（Ｓ４−４）、過大でない場合には、マニ
ピュレータ関節駆動部によりマニピュレータを駆動する
（Ｓ４−１２）。一方、各関節の角速度指令値が過大で
ある場合には、最大遅れ量を超えているか否かを判定し
（Ｓ４−５）、最大遅れ量を超えている場合には、第１
実施例で説明した絶対値減速手段により絶対値の減速を
行い（Ｓ４−６）、マニピュレータ関節駆動部によりマ
ニピュレータを駆動する（Ｓ４−１２）。また、最大遅
れ量を超えていない場合には、関節速度減算手段により
各関節毎の角速度指令値を減算して（Ｓ４−７）、マニ
ピュレータ関節駆動部によりマニピュレータを駆動する
（Ｓ４−１２）。また、絶対値減速手段からの各関節の
角速度指令値は、順変換部により順変換されて手先速度
実行ベクトルが演算される（Ｓ４−８）。この手先速度
実行ベクトルが、実行ベクトル積算手段（実現位置姿勢
計算手段）により積算されて実現位置姿勢が得られる
（Ｓ４−９）。また、手先指令生成部により生成された
手先速度指令ベクトルが、指令ベクトル積算手段（目標
位置姿勢計算手段）により積算されて目標位置姿勢が得
られる（Ｓ４−１０）。そして、実現位置姿勢と目標位
置姿勢とが遅れベクトル計算手段に入力されて、積算遅
れベクトル（差）が計算される（Ｓ４−１１）。この積
算遅れベクトルは、先に説明したように、追従ベクトル
算出手段に入力され追従ベクトルの算出に使用される。

【００２２】＜第５実施例＞図５、図６は、第５実施例
に係る垂直多関節型マニピュレータの制御装置における
制御方法を示すフローチャートである。第５実施例に係
る垂直多関節型マニピュレータの制御装置は、ノズルや
溶接トーチのようにツールの姿勢が定義できる場合の制
御装置である。すなわち、第３実施例では、関節角速度
の過大指令に対して各関節軸毎に独立で減速していた
が、第５実施例では、ツール軸方向と指令ベクトルの並
進移動速度方向ベクトルに直交する迂回ベクトルを生成
し、元の指令ベクトルに加えて修正された指令ベクトル
とすることにより、ツールの姿勢に影響なく特異点に接
近することを回避する構成となっている。ただし、積算
迂回量が大きくなり過ぎると、マニピュレータ動作が作
業で許容される誤差範囲を突破するので、最大迂回量を
事前に設定しておき、これ以上の積算迂回が発生した場
合には、第１実施例において説明した減速式の回避法へ
切り替える方法を含むこととする。

【００２３】具体的には、図５に示すように、手先指令
生成部により手先速度指令ベクトルを生成し（Ｓ５−
１）、この手先速度指令ベクトルに基づいて追従ベクト
ル算出手段により追従ベクトルを算出する。すなわち、
手先速度指令ベクトルに対して遅れベクトル積算手段に
より演算した積算遅れベクトル（マイナス値）を加算す
る（Ｓ５−２）。続いて、算出した追従ベクトルを逆変
換部により逆変換して各関節の角速度指令値を演算し
（Ｓ５−３）、切替手段により手先速度指令ベクトルの
修正を行うための手段を切り替える。すなわち、過大指
令検出手段により各関節の角速度指令値が過大であるか
否かを判定し（Ｓ５−４）、過大でない場合には、マニ
ピュレータ関節駆動部によりマニピュレータを駆動する
（Ｓ５−１３）。一方、各関節の角速度指令値が過大で
ある場合には、最大遅れ量を超えているか否かを判定し
（Ｓ５−５）、最大遅れ量を超えている場合には、第１
実施例で説明した絶対値減速手段により絶対値の減速を
行い（Ｓ５−７）、マニピュレータ関節駆動部によりマ
ニピュレータを駆動する（Ｓ５−１３）。また、最大遅
れ量を超えていない場合には、迂回ベクトル計算手段に
より迂回ベクトルを計算する（Ｓ５−６）。すなわち、
迂回を中止する場合には、第１実施例で説明した絶対値
減速手段により絶対値の減速を行い（Ｓ５−７）、マニ
ピュレータ関節駆動部によりマニピュレータを駆動する
（Ｓ５−１３）。また、迂回ベクトル計算手段により各
関節の角速度指令値が計算された場合には、この角速度
指令値に基づいて、マニピュレータ関節駆動部によりマ
ニピュレータを駆動する（Ｓ５−１３）。なお、この迂
回ベクトル計算手段における計算（Ｓ５−６）は、後に
図６を参照して詳述する。また、絶対値減速手段からの
各関節の角速度指令値は、順変換部により順変換されて
手先速度実行ベクトルが演算される（Ｓ５−８）。この
手先速度実行ベクトルが、実行ベクトル積算手段（実現
位置姿勢計算手段）により積算されて実現位置姿勢ベク
トルが得られる（Ｓ５−９）。また、手先指令生成部に
より生成された手先速度指令ベクトルが、指令ベクトル
積算手段（目標位置姿勢計算手段）により積算されて目
標位置姿勢が得られる（Ｓ５−１０）。そして、実現位
置姿勢と目標位置姿勢とが遅れベクトル計算手段に入力
されて、積算遅れベクトル（差）が計算される（Ｓ５−
１１）。この積算遅れベクトルは、先に説明したよう
に、追従ベクトル算出手段に入力され追従ベクトルの算
出に使用される。さらに、実現位置姿勢はツール方向ベ
クトル更新手段に入力されてツール方向ベクトルが更新
され（Ｓ５−１２）、迂回ベクトル計算手段における計
算に使用される。

【００２４】迂回ベクトル計算手段における、迂回ベク
トルの計算は図６に示すように、各関節の角速度指令値
が過大であり、かつ最大遅れ量を超えている場合に、ツ
ール方向ベクトルと追従ベクトルの並進成分とのベクト
ル外積を求め（Ｓ６−１）、この外積ベクトルの絶対値
が「０」であるか否かを判定する（Ｓ６−２）。ここで
外積ベクトルの絶対値が「０」である場合には、事前に
設定した方向ベクトルを迂回ベクトルの方向として使用
し（Ｓ６−３）、外積ベクトルの絶対値が「０」でない
場合には、外積ベクトルの方向を迂回ベクトルの方向と
して使用する。（Ｓ６−４）。この時、ベクトル外積計
算で得られる方向ベクトルと、反対方向の、ないしはマ
イナス１を乗じた方向ベクトルの２つを迂回ベクトルの
方向ベクトルとし、以下どちらかを選択する手順までの
計算は２つのベクトルについて実施する物とする。続い
て、迂回ベクトルの大きさを最小値に設定する。（Ｓ６
−５）

【００２５】手先指令ベクトルに基づいて迂回ベクトル
と追従ベクトルの和ベクトルを求め（Ｓ６−７）、逆変
換部により逆変換して各関節の角速度指令値を演算する
（Ｓ６−８）。続いて、過大指令を検出したか否かを判
定する（Ｓ６−９）。なお。過大指令の検出では、２個
のベクトルのうち、変換後の関節角速度が、より小さく
なるベクトルを採用して判定を行う。ここで、過大指令
を検出しない場合には、各関節の角速度指令値を演算し
（Ｓ６−１１）、図５に示すように、マニピュレータ関
節駆動部によりマニピュレータを駆動する（Ｓ５−１
３）。一方過大指令を検出した場合には、迂回ベクトル
の大きさを、事前に設定しておいた増分値で増分し（Ｓ
６−１１）、迂回ベクトルが最大値であるか否かを判定
する（Ｓ６−６）。迂回ベクトルが事前に設定した最大
値より大きければ、図５に示すように、第１実施例で説
明した絶対値減速手段により、手先指令ベクトルの絶対
値の減速を行い（Ｓ５−７）、マニピュレータ関節駆動
部によりマニピュレータを駆動する（Ｓ５−１３）。一
方、迂回ベクトルが最大値に達しない場合には、和ベク
トルの演算まで戻り、以降の処理を行う。図９は、第５
実施例におけるマニピュレータ先端のツール方向ベクト
ルと、指令ベクトルの並進移動速度方向ベクトルと、迂
回ベクトルの関係を示す模式図である。図９では、実際
のロボット・マニピュレータでよく使われる同軸手首構
造において発生しやすい特異姿勢に向かう状況を示して
いる。図１０は、第５実施例における迂回経路の概要を
示す模式図である。第５実施例では、図１０に示すよう
に、特異姿勢状態となる場合に、ツール方向および並進
方向と直交する方向に迂回ベクトルを設定し、マニピュ
レータ先端を迂回させることにより、特異姿勢状態を迂
回することができる。

【００２６】

【発明の効果】以上説明したように、本発明に係る垂直
多関節型マニピュレータの制御装置によれば、順変換、
逆変換を持つ従来の教示再生型ロボットの制御手法を利
用して、実時間追値制御である操縦型ロボット・マニピ
ュレータの特異点近傍における良好な操作性を確保する
ことができる。すなわち、既存のロボット制御系を有効
に使うことができるため、製造コストを低減することが
できる。例えば、最も一般的なPUMA型と呼ばれる同軸手
首構造を有する垂直６軸多関節では、手先直交軸と各関
節角座標の順変換部、逆変換部は多くの研究で公知の技
術となっており、ロボット・マニピュレータの製造メー
カー、ユーザー共に制御則、制御装置に関する知識が深
いので、本発明に係る垂直多関節型マニピュレータの制
御装置を追加的に適合させることが可能である。

【００２７】ところで、多関節マニピュレータの特異
点、特異姿勢における動作は、各関節の回転速度が有限
であるため、特異点に極めて近い点を通過する際には、
補間された手先動作は、速度が低下せざるを得ない。こ
れは、制御手法の問題では無く幾何学的な運動軌跡の精
度と速度のバランスの問題である。しかし、多くの操縦
型マニピュレータの適用作業では、運動軌跡の誤差やバ
ラツキの許容量が、通常のマニピュレータの制御の誤
差、バラツキより大きい場合もあり、このような場合に
は誤差の許容量の範囲内で指令値を変更して、手先速度
の低下を防止した方が作業効率上有効な場合もある。こ
の点、本発明に係る垂直多関節型マニピュレータの制御
装置によれば、特異点近傍におけるマニピュレータの挙
動に対して、動作速度の安定性を優先して軌跡の変化量
を大きくするか、あるいは軌跡の変化が少なく大きく減
速するように構成するか、制御方法と事前設定の係数で
調整することが可能である。また、本発明に係る垂直多
関節型マニピュレータの制御装置は、６軸垂直多関節型
ロボット・マニピュレータ、同軸手首構造を含んで冗長
自由度マニピュレータ、６軸以下の自由度のマニピュレ
ータにも適用することが可能である。また、本発明に係
る垂直多関節型マニピュレータの制御装置は、順変換、
逆変換を多用しているので計算負荷は大きいが、最近の
計算機の性能向上により、安価な汎用計算機を利用して
も実用的な制御周期の間にこれらのアルゴリズムを実行
することが可能である。

【図面の簡単な説明】

【図１】第１実施例に係る垂直多関節型マニピュレータ
の制御装置における制御方法を示すフローチャートであ
る。

【図２】第２実施例に係る垂直多関節型マニピュレータ
の制御装置における制御方法を示すフローチャートであ
る。

【図３】第３実施例に係る垂直多関節型マニピュレータ
の制御装置における制御方法を示すフローチャートであ
る。

【図４】第４実施例に係る垂直多関節型マニピュレータ
の制御装置における制御方法を示すフローチャートであ
る。

【図５】第５実施例に係る垂直多関節型マニピュレータ
の制御装置における制御方法を示すフローチャートであ
る。

【図６】第５実施例に係る垂直多関節型マニピュレータ
の制御方法（迂回ベクトル計算処理）を示すフローチャ
ートである。

【図７】従来の操縦型ロボット・マニピュレータの制御
方法を示すフローチャートである。

【図８】本発明の実施形態に係る垂直多関節型マニピュ
レータの制御装置の構成を示すブロック図である。

【図９】第５実施例におけるマニピュレータ先端のツー
ル方向ベクトルと、指令ベクトルの並進移動速度方向ベ
クトルと、迂回ベクトルの関係を示す模式図である。

【図１０】第５実施例における迂回経路の概要を示す模
式図である。

【符号の説明】

１０ 制御部 １１ 各関節角駆動制御部 １２ 特異点回避制御部 ２０ 操作者 ３０ 手先指令生成部 ４０ マニピュレータ・メカニズム ５０ マニピュレータ用動力盤

───────────────────────────────────────────────────── フロントページの続き Ｆターム(参考） 3C007 BS10 JT06 JU15 LT01 LU01 MT00 5H269 AB33 BB12 CC09 EE01 NN04

Claims (5)

【特許請求の範囲】
1. 【請求項１】 操作者の操作入力に基づいて、マニピュ
   レータ先端の位置および姿勢に対する直交座標系におけ
   る速度および角速度指令である手先速度指令ベクトルを
   連続的に生成するための手先指令生成部と、 前記手先指令生成部により生成された前記手先速度指令
   ベクトルを前記マニピュレータの各関節の関節角速度指
   令値に変換するための逆変換部と、 前記関節角速度指令値に基づいて前記マニピュレータの
   関節を駆動するためのマニピュレータ関節駆動部と、 各関節の関節角速度値に基づいて手先速度ベクトルを求
   めるための順変換部とを有する多自由度の垂直多関節型
   マニピュレータの制御装置において、 各関節に対する前記関節角速度指令の絶対値が、各関節
   毎に事前に設定した関節角速度最大値を超えることを検
   出するための過大指令検出手段と、 前記過大指令検出手段により過大指令が検出された場合
   に、前記手先速度指令ベクトルの絶対値が前記関節角速
   度最大値を超えないように減算して、前記手先速度指令
   ベクトルを変更するための手先速度減算手段と、 前記手先速度減算手段により減算された手先速度指令ベ
   クトルに基づいて、前記逆変換部で変換された前記関節
   角速度指令値の絶対値が、事前に設定した関節角速度最
   小値を下回ることが検出された場合に、前記マニピュレ
   ータを一時停止させるとともに、操作者に対して特異点
   接近を表示するための過小指令検出手段とを含んで構成
   される絶対値減速手段を備えたことを特徴とする垂直多
   関節型マニピュレータの制御装置。
2. 【請求項２】 前記絶対値減速手段において、 前記手先速度減算手段は、事前に設定された減衰率を前
   記手先速度指令ベクトルに乗じて求める方式で構成さ
   れ、 前記過小指令検出手段は、前記手先速度減算手段におけ
   る演算結果を前記過大指令検出手段により過大指令であ
   るか否かを検出し、過大指令ではないことが検出された
   場合に、当該演算結果を減算された手先速度指令ベクト
   ルとし、過大指令であることが検出された場合に、前記
   減衰率を既に減算された手先速度指令ベクトルに乗ずる
   過程を繰り返し、該繰り返し数が事前に設定された最大
   減衰回数を上回っても前記関節角速度指令値が関節角速
   度最大値より大きい場合に、前記マニピュレータを一時
   停止させるとともに、操作者に対して特異点接近を表示
   することを特徴とする請求項１記載の垂直多関節型マニ
   ピュレータの制御装置。
3. 【請求項３】 前記過大指令検出手段により過大指令が
   検出された場合に、前記関節角速度最大値を超える関節
   についてだけ、前記関節角速度指令値の絶対値が前記関
   節角速度最大値を超えないように減算するための関節速
   度減算手段と、 前記関節速度減算手段により減算された関節角速度を含
   めた全ての関節角速度に基づいて前記順変換部で求めら
   れる手先速度実行ベクトルと、前記手先速度指令ベクト
   ルの差を求めるための遅れベクトル計算手段と、 前記遅れベクトル計算手段により求められる遅れベクト
   ルを制御開始以降の時間で積算するための遅れベクトル
   積算手段と、 前記過大指令検出手段により前記関節角速度指令値の絶
   対値が前記関節角速度最大値を超えないことが検出され
   た場合に、前記積算遅れベクトルと反対方向の復帰ベク
   トルを求めるとともに、前記手先速度指令ベクトルと前
   記復帰ベクトルとの和ベクトルを求めるための追従ベク
   トル算出手段と、 前記遅れベクトル積算手段により積算される積算遅れベ
   クトルの絶対値が、事前に設定した最大遅れ量を超える
   ことが検出され、かつ前記過大指令検出手段により過大
   指令が検出された場合に、前記手先速度指令ベクトルの
   修正を行うための手段を、前記関節速度減算手段から前
   記絶対値減速手段に切り替えるための切替手段とを有す
   ることを特徴とする請求項１記載の垂直多関節型マニピ
   ュレータの制御装置。
4. 【請求項４】 前記過大指令検出手段により過大指令が
   検出された場合に、前記関節角速度最大値を超える関節
   について、前記関節角速度指令値の絶対値が前記関節角
   速度最大値を超えないように減算するための関節速度減
   算手段と、 前記関節速度減算手段により更新された関節角速度を含
   めた全ての関節角速度に基づいて前記順変換部で手先速
   度実行ベクトルを求め、この手先速度実行ベクトルの制
   御動作を開始した以降、積分して得られる現在の実現さ
   れている位置姿勢を計算するための実現位置姿勢計算手
   段と、 前記手先指令生成部により生成される前記手先速度指令
   ベクトルの制御動作を開始した以降、積分して得られる
   現在の目標位置姿勢を計算するための目標位置姿勢計算
   手段と、 前記目標位置姿勢計算手段により求められる目標位置姿
   勢ベクトルと、前記実現位置姿勢計算手段により求めら
   れる実現位置姿勢ベクトルの差を求めるための積算遅れ
   ベクトル計算手段と、 前記過大指令検出手段により前記関節角速度指令値の絶
   対値が前記関節角速度最大値を超えないことが検出され
   た場合に、前記積算遅れベクトルと反対方向の復帰ベク
   トルを求めるとともに、前記手先速度指令ベクトルと前
   記復帰ベクトルとの和ベクトルを求めるための追従ベク
   トル算出手段と、 前記遅れベクトル積算手段により積算される積算遅れベ
   クトルの絶対値が、事前に設定した最大遅れ量を超える
   ことが検出され、かつ前記過大指令検出手段により過大
   指令が検出された場合に、前記手先速度指令ベクトルの
   修正を行うための手段を、前記関節速度減算手段から前
   記絶対値減速手段に切り替えるための切替手段とを有す
   ることを特徴とする請求項１記載の垂直多関節型マニピ
   ュレータの制御装置。
5. 【請求項５】 マニピュレータ先端の姿勢に対して手先
   ツール姿勢を定義可能な場合に、前記マニピュレータ先
   端姿勢に応じて決定される手先ツール姿勢を示す方向ベ
   クトルを更新計算するためのツール方向ベクトル更新手
   段と、 前記過大指令検出手段により過大指令が検出された場合
   に、前記ツール姿勢に応じて決定されるツール方向ベク
   トルと、前記手先速度指令ベクトルの並進移動成分との
   外積ベクトルと平行な方向ベクトル、ないしは前記外積
   ベクトルの大きさが零の場合に、事前に設定しておいた
   方向ベクトルであり、前記過大指令検出手段により過大
   指令とならない最小の大きさの迂回ベクトルを算出し、
   前記手先速度指令ベクトルとの和ベクトルに基づいて回
   避動作を行うための迂回ベクトル計算手段と、 前記迂回ベクトル計算手段により更新された関節角速度
   を含めた全ての関節角速度に基づいて前記順変換部で手
   先速度実行ベクトルを求め、この手先速度実行ベクトル
   の制御動作を開始した以降、積分して得られる現在の実
   現されている位置姿勢を計算するための実現位置姿勢計
   算手段と、 前記手先指令生成部により生成される前記手先速度指令
   ベクトルの制御動作を開始して以降、積分して得られる
   現在の目標位置姿勢を計算するための目標位置姿勢計算
   手段と、 前記目標位置姿勢計算手段で求められる目標位置姿勢ベ
   クトルと、前記実現位置姿勢計算手段で求められる実現
   位置姿勢ベクトルの差を求めるための積算遅れベクトル
   計算手段と、 前記過大指令検出手段により関節角速度指令値の絶対値
   が関節角速度最大値を超えないことが検出された場合
   に、前記積算遅れベクトルと反対方向の復帰ベクトルを
   求めるとともに、前記手先速度指令ベクトルと前記復帰
   ベクトルとの和ベクトルを求めるための追従ベクトル算
   出手段と、 前記遅れベクトル積算手段により積算される積算遅れベ
   クトルの絶対値が、事前に設定した最大遅れ量を超える
   ことが検出され、かつ前記過大指令検出手段により過大
   指令が検出された場合、および前記迂回ベクトル計算手
   段により迂回ベクトルが事前に設定した最大値となって
   も前記過大指令検出手段により過大角速度指令が発生す
   る場合に、前記手先速度指令ベクトルを修正するための
   手段を、前記迂回ベクトル計算手段から前記絶対値減速
   手段に切り替えるための切替手段を有することを特徴と
   する請求項１記載の垂直多関節型マニピュレータの制御
   装置。