JP5343725B2 - ロボット制御装置 - Google Patents

Description

本発明は、ロボット制御装置に係り、特に、複数の各関節についてのイナーシャ値を用いてロボットの移動軌跡を制御する場合において、そのイナーシャ値の取得に関するものである。

ロボットは、動作プログラムにより定められる移動軌跡に沿って移動するように制御される。特に、移動軌跡精度が作業精度に影響を及ぼす場合には、ロボットの移動軌跡は動作プログラムによって定められる軌跡からずれないように、高い軌跡精度を得ることが求められる。しかしながら、ロボットは、入力信号に対して遅れて動作する遅れ系であるため、精度の良い移動制御をすることが難しくなってくる。
このような動作遅れを防止するためのシステムとして、例えば特許文献１にみられるような逆システムがある。

特開平５−１４３１０６号公報

上記特許文献１に開示された逆システムを用いれば、入力信号に対して遅れの少ない制御を行うことができるので、ロボットを動作プログラムによる移動軌跡に沿って精度良く移動させることができる。
しかしながら、逆システムで演算のために用いられるアームのイナーシャ値は、通常、一連の動作の中での平均的な姿勢でのイナーシャ値が使用されるので、特に、曲率半径の小さな弧状軌跡部分では、遅れが現れやすくなって動作プログラムにより定められる移動軌跡から外れてしまい、直線や曲率半径の大きな移動軌跡部分に比べて軌跡精度が低下する。

本発明は上記の事情に鑑みてなされたもので、その目的は、曲率半径の小さな弧状軌跡部分でも、軌跡精度を高くすることができるイナーシャ値を求めようとするもので、そのためのロボット制御装置におけるイナーシャ取得方法およびロボット制御装置を提供することにある。

本発明では、複数のアームを順次関節により連結して構成されたロボットを制御対象とするロボット制御装置は、複数の各アームの目標位置が入力されると、各アームについて、所定の演算式により目標位置を修正した修正目標位置を演算する逆システム演算手段と、この逆システム演算手段から修正目標位置が与えられると、各アームを駆動するアクチュエータを制御して各アームが修正目標位置に移動するように制御する制御手段とを備えている。

そして、請求項１および３の発明では、特定の弧状軌跡に沿ってアーム先端が移動するとき、当該弧状軌跡を対象に、各サンプリング位置でのイナーシャと、次のサンプリング位置までのアーム先端の移動距離とを乗じてイナーシャ・距離積を求め、このイナーシャ・距離積の総和を、イナーシャ・距離積を求めたサンプリング位置相互間におけるアーム先端の移動距離の総和で除して平均イナーシャ値を求め、この平均イナーシャ値を用いて逆システム演算手段で目標位置から修正目標位置を演算するようにしたので、軌跡精度が向上する。

請求項２および４の発明では、隣どうしのサンプリング地点における２つの速度ベクトルのなす角度が所定角度以上となる範囲を、平均イナーシャ値を求めるための前記特定の弧状軌跡とするので、ユーザが面倒な弧状軌跡の指定操作を行う必要がない。

本発明の一実施形態おけるロボットの電気的構成を示すブロック図 ロボットのトルク伝達を示すブロック線図 アームのイナーシャを求めるためのフローチャート サンプリング地点でのアーム先端の位置と速度を示す図 回転関節の軸を中心とするアームイナーシャの計算を説明するための図 異なる姿勢でのアームイナーシャの違いを説明するための図 一つのアームの重心位置と重心位置を中心とするイナーシャの説明図 モータからアームへのトルク伝達を説明するための図 ロボット装置を示す斜視図

以下、本発明の一実施形態を図面に基づいて説明する。図９には、産業用ロボット装置１が示されている。この産業用ロボット装置１は、ロボット２と、このロボット２を制御するロボット制御装置３と、このロボット制御装置３に接続されティーチング作業を行なう際に使用するティーチングペンダント（操作手段）４とから構成されている。このティーチングペンダント４は、各種の操作部４ａおよび表示器４ｂを備えている。

ロボット２は、例えば６軸の垂直多関節型ロボット１からなるもので、ベース５と、このベース５に水平方向に旋回可能に支持されたショルダ部６と、このショルダ部６に上下方向に旋回可能に支持された下アーム７と、この下アーム７に上下方向に旋回可能に支持された第１の上アーム８と、この第１の上アーム８の先端部に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に上下方向に回転可能に支持された手首１０と、この手首１０に回転（捻り動作）可能に支持されたフランジ１１とから構成されている。

ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１は、ロボットにおけるアームとして機能し、ベース５を除く各アームは、下段のアームに対し関節、例えば回転関節によって順次回転可能に連結されている。なお、アーム先端であるフランジ１１には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。

上述のアームとアームとを連結する回転関節は、図８に示すように、前段のアームに軸１２を軸受（図示せず）により回転可能に支持する周知の回転軸支持構造からなるもので、後段のアームが軸１２と一体的に回転するように構成される。そして、例えば前段のアーム側に固定されたモータ（アクチュエータ）１３の回転軸１３ａが減速装置１４の入力軸１４ａに連結されていると共に、減速装置１４の出力軸１４ｂが軸１２に連結されている。これにより、モータ１３の回転軸１３ａの回転が減速装置１４により減速されて軸１２に伝達され、次段のアームが回転関節の回転中心軸線（軸１２の中心軸線）Ｌｃを中心にして回転するようになっている。

ここで、第１のアームであるベース５と第２のアームであるショルダ部６との間を連結する回転関節の回転中心軸を第１軸Ｌｃ−１、第２のアームであるショルダ部６と第３のアームである下アーム７との間を連結する回転関節の回転中心軸を第２軸Ｌｃ−２、第３のアームである下アーム７と第４のアームである第１の上アーム８との間を連結する回転関節の回転中心軸を第３軸Ｌｃ−３、第４のアームである第１の上アーム８と第５のアームである第２の上アーム９との間を連結する回転関節の回転中心軸を第４軸Ｌｃ−４、第５のアームである第２の上アーム９と第６のアームである手首１０との間を連結する回転関節の回転中心軸を第５軸Ｌｃ−５、第６のアームである手首１０と第７のアームであるフランジ１１との間を連結する回転関節の回転中心軸を第６軸Ｌｃ−６と称することとする。

ロボット２の動作を制御するロボット制御装置３は、図１に示すように、ＣＰＵ（制御手段）１５と、駆動回路１６と、位置検出回路１７、逆システム演算回路（逆システム演算手段）１８とを備えている。コンピュータシステムの主要部を構成するＣＰＵ１５には、ロボット２全体のシステムプログラムや動作プログラムを作成するためのロボット言語などを記憶するＲＯＭ１９及びロボット２の動作プログラムなどを記憶するＲＡＭ２０が接続されていると共に、前記ティーチングペンダント４が接続されている。なお、モータ１３は、各アーム６〜１１について一対一の関係で設けられているが、図１では、モータ１３は１台だけ図示した。

位置検出回路１４には、各アーム６〜１１の駆動源である各モータ１３の回転軸１３ａに連結されたロータリエンコーダ（回転位置検出手段）２１が接続されている。この位置検出回路１４は、ロータリエンコーダ２１から入力される検出信号に基づいてモータ１３の回転位置を検出し、その回転位置情報をＣＰＵ１５および駆動回路１６に与える。駆動回路１６は、逆システム演算回路１８から与えられる位置指令と、位置検出回路１７から与えられる回転位置とを比較し、その差に応じたトルクを発生するようにモータ１３に供給する電流を制御する。

ＣＰＵ１５は、位置検出回路１４から与えられる各モータ１３の回転位置情報から、ベース５に対するショルダ部６の回転角度、ショルダ部６に対する下アーム７の旋回角度、下アーム７に対する第１の上アーム８の旋回角度、第１の上アーム８に対する第２の上アーム９の捻り回転角度、第２の上アーム９に対する手首１０の捻り旋回角度、手首１０に対するフランジ１１の捻り回転角度を検出する。ちなみに、上記の各アーム６〜１１の回転(旋回)角度は、ロータリエンコーダ２１などのモータ角度検出手段からの検出値を角度に変換することによって求める。

ベース５および各アーム６〜１１には、３次元の座標が規定されている。このうち、床面に据え付けられるベース５の座標系は、不動の座標系としてロボット２の基準座標とされるものであり、ベース５の下端中心を原点としている。アーム６〜１１の座標系は、各アーム６〜１１の先端部に規定されており、各回転関節の回転により基準座標上での位置と姿勢が変化する。ＲＯＭ１９には、基準座標上におけるショルダ部６の座標位置、ショルダ部６の座標上における下アーム７の座標位置、下アーム７の座標上における第１の上アーム８の座標位置、第１の上アーム８の座標上における第２の上アーム９の座標位置、第２の上アーム９の座標上における手首１０の座標位置、手首１０の座標上におけるフランジ１１の座標位置、および各アーム６〜１１の長さが記憶されている。そして、ＣＰＵ１５は、各アーム６〜１１の座標の位置と向きとを、各アーム６〜１１の回転位置情報と各アーム６〜１１の長さ情報と各アーム６〜１１の座標の相互位置関係から、座標変換の計算機能により基準座標上での位置と姿勢とに変換して認識する。なお、座標の姿勢は、各アーム６〜１１のＸ，Ｙ，Ｚの各座標軸上における単位ベクトルの基準座標上での向きで表される。

また、逆に、ＣＰＵ１５は、動作プログラムからアーム先端であるフランジ１１の座標の基準座標上での位置と姿勢が与えられると、各アーム６〜１１のモータ１３の回転位置を制御することにより、各アーム６〜１１の座標のロボット座標上での位置と姿勢を制御してフランジ１１が動作プログラム通りの位置と姿勢を取るように各アーム６〜１１を動作させる。つまり、各アーム６〜１１の位置と姿勢に対して各アーム６〜１１のモータ１３の回転位置が一義的に定まるので、ＣＰＵ１５は、各アーム６〜１１の位置と姿勢に対する各アーム６〜１１のモータ１３の回転位置を目標位置として出力することにより、各アーム６〜１１が動作プログラムから与えられる位置と姿勢を取るように制御するものである。

さて、逆システム演算回路１８は、アーム先端を動作プログラムから与えられる位置に制御するために、制御対象である各アーム６〜１１の目標位置（座標の位置で示される。）に対応した各アーム６〜１１のモータ１３の目標位置が入力されたとき、駆動回路１６から各アーム６〜１１の先端（座標が規定された位置）までの伝達関数が１となるような位置指令を出力する。そのために、逆システム演算回路１８は、駆動回路１６から各アーム６〜１１のモータ１３を経てアーム先端（フランジ１１）に至るまでの伝達関数の逆数を演算式として有し、各アーム６〜１１のモータ１３の目標位置が入力されたとき、この目標位置を上記演算式により修正して当該修正後の目標位置（修正目標位置）を位置指令として駆動回路１６に与えるものである。すると、駆動回路１６から各アーム６〜１１のモータ１３からフランジ１１に至るまでの各伝達経路が、それぞれ伝達関数を「１」とするような制御システムとなるので、軌跡精度の高い制御を行うことができるものである。

上記の逆システム演算回路１８が有する演算式を求めるには、まず、駆動回路１６から各アーム６〜１１のモータ１３を経てフランジ１１に至るまでの制御システム全体の伝達関数Ｇ（ｓ）を求めなくてはならない。図２は、駆動回路１６から各アーム６〜１１のモータ１３を経てフランジ１１に至るまでの複数の制御システムのうち、一つのアームについての制御システムをブロック線図に示したものである。この制御システムのうち、サーボ駆動系は、ロータリエンコーダ２１と、ロータリエンコーダ２１の検出信号を処理してモータ１３の現在位置を取得するための位置検出回路１７と、修正目標位置とモータ１３の現在位置１２との差に応じた電流をモータ１３に供給するパワーアンプを備えた駆動回路１６とからなるが、このサーボ駆動系は、設計者が実装する部分であるから、その伝達関数は既知である。しかし、各モータ１３からアーム先端（フランジ１１）に至るまでのトルク伝達経路の伝達関数はアームの速度や姿勢によって変化するので未知である。

このトルク伝達経路の伝達関数を、図２を参照して求める。図２において、サーボ系によりモータ１３に電流が供給されると、モータ１３はその電流に応じたトルクＴｉを発生する。このトルクＴｉにより、モータ１３の回転軸１３ａは、Ｔｉ／（Ｊｍ・ｓ＋ｄｍ）の回転速度で回転する。この回転軸１３ａの回転は、減速装置１４により１／ｇに減速されて当該減速装置１４の出力軸１４ｂからアームへと伝えられる。なお、Ｊｍはモータ１３のイナーシャ、ｓはラプラス演算子、ｄｍはモータの粘性、ｇは減速装置の減速比である。

モータ１３の回転速度は、サーボ系に速度フィードバックとして与えられ、サーボアンポは、指令されたモータ１３の位置と、フィードバックされた回転速度を微分して得られるモータ１３の現在位置とを比較してその差分に応じた電流値をモータ１３に供給する。
減速装置１４の出力軸１４ｂの回転速度は、後述のようにして取得されるフランジ１１の現在の回転速度を加減算した後に積分（１／ｓ）される。この積分値は、出力軸１４ｂにより直接回転されるアームからフランジ１１までの全てのアームを一体化された１本のアームと仮定した場合において、出力軸１４ｂからフランジ１１までの捩れ角Δθに相当する。そして、この捩れ角Δθに上記一体化された１本のアームのばね定数Ｋｓを乗じて当該出力軸１４ｂからアーム側へ伝達されるトルクＴｏを求める。この伝達トルクＴｏは、モータ１３の回転軸１３ａに対し、上記一体化された１本のアームを回転させる際の反力として作用するが、回転軸１３ａには減速装置１４の減速比１／ｇ倍となって伝えられる。

減速装置１４の出力軸１４ｂの出力トルクＴｏによって上記一体化された１本のアームが回転される。このときの上記一体化された１本のアーム（フランジ１１）の回転速度は、Ｔｏ／（Ｊａ・ｓ＋ｄａ）によって求められる。ここで、Ｊａは上記一体化された１本のアームのイナーシャ、ｓはラプラス演算子、ｄａは上記一体化された１本のアームの粘性である。このようにして求められたフランジ１１の回転速度は、減速装置１４の出力軸１４ｂからフランジ１１までの捩れ角Δθを求める際に、前述したように減速装置１４の出力軸１４ｂの回転速度と加減算される。そして、上記のフランジ１１の回転速度は積分（１／ｓ）され、この積分値がフランジ１１の現在位置として求まる。

以上のようなモータ１３のトルクの伝達経路において、その伝達関数を決定する要素のうち、モータ１３のイナーシャＪｍおよび粘性係数ｄｍ、減速装置１４の減速比ｇ、上記一体化された１本のアームのばね定数Ｋｓは、一定値で既知ある。しかしながら、上記一体化された１本のアームとしてみたときのイナーシャＪａは、ロボット２の動作毎に変化する。例えば、ショルダ部６と下アーム７との間を連結する回転関節の回転中心軸である第２軸Ｌｃ−２を中心とするアームのイナーシャＪａは、第２軸ｉ−２より先にある全てのアーム（下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１）を一体化された１本のアームと仮定して求めるが、ロボット２が図６（ａ）に示す姿勢にあるときと、同図（ｂ）に示す姿勢にあるときとでは、異なる値を示す。このため、モータ１３のトルクの伝達経路の伝達関数をより正しく求めようとすると、各回転関節の回転中心について、当該回転中心よりもフランジ１１側に存する全てのアームを一体化された１本のアームと仮定して、当該１本のアームのイナーシャを時々刻々変化するロボット２の姿勢毎に求める必要がある。

アームのイナーシャを時々刻々変化するロボット２の姿勢毎に求めることは、可能である。しかしながら、そのような時々刻々変化するイナーシャ値を使用することで、修正位置指令が不連続となってしまい、滑らかな動きができなかったり、モータの限界を超えた指令値が出ることでモータが停止してしまったりするという不具合を生ずる。
そこで本実施形態では、ロボット２の一連の動作において、フランジ１１の移動軌跡のうち、所定の曲率半径以下の円弧軌跡で移動する際のイナーシャの平均を求め、これを用いてトルク伝達経路の伝達関数を演算する構成とした。各回転関節の軸Ｌｃを回転中心とするアームのイナーシャＪａを求める演算を容易にするために、各アームについて、質量Ｍと、重心の位置（各アームの座標上の位置）Ｐｇと、重心を回転中心とするイナーシャＪｃとが記憶手段としてのＲＯＭ１９に記憶されている。

図７に示すように、重心位置Ｐ_Ｇがアーム自身の回転中心Ｌｃからｒだけ離れた位置にあるアームのイナーシャＪは、次の（１）式で求めることができる。
Ｊ＝Ｍｒ^２＋Ｊｃ……（１）
但し、Ｍはアームの質量である。

したがって、回転関節が指定されたとき、その回転関節の先にアームが１つしか存在しない場合、上記指定された回転関節の軸を回転中心とするアームのイナーシャは、上記（１）式により求めることができる。また、回転関節が指定されたとき、その回転関節の先にアームが複数存在する場合、上記指定された回転関節の軸を回転中心とするそれら複数のアームのイナーシャは、上記（１）式により求めた各アームのイナーシャの総和として求めることができる。

この場合において、図５（ａ）に示すように、軸Ｌｃ−１を回転中心とするアームのイナーシャＪａは、当該軸Ｌｃ−１の回転により動かされる全てのアーム、つまり軸Ｌｃ−１の回転によって直接回転されるアームを含めて当該アームよりもフランジ１１側に存する全てのアーム、即ちショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１を１本のアームと仮定して、上記（１）式により各アーム６〜１１についてのイナーシャを演算し、それらイナーシャの総和をＪａとする。なお、軸Ｌｃ−１と各アーム６〜１１の重心までの距離をｒ６〜ｒ１１として図５（ａ）に示した。

また、図５（ｂ）に示すように、軸Ｌｃ−２を回転中心とするアームのイナーシャＪａは、当該軸Ｌｃ−２の回転により直接動かされるアームを含めて当該アームよりもフランジ１１側に存する全てのアーム、即ち下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１を１本のアームと仮定して、上記（１）式により各アーム７〜１１についてのイナーシャを演算し、それらイナーシャの総和をＪａとする。なお、軸Ｌｃ−２と各アーム７〜１１の重心までの距離をｒ７〜ｒ１１として図５（ｂ）に示した。

更に、図５（ｃ）に示すように、軸Ｌｃ−５を回転中心とするアームのイナーシャＪａは、当該軸Ｌｃ−５よりもフランジ１１側に存する全てのアーム、即ち手首１０、フランジ１１を１本のアームと仮定して、上記（１）式により各アーム１０，１１についてのイナーシャを演算し、それらイナーシャの総和をＪａとする。なお、軸Ｌｃ−５と各アーム７〜１１の重心までの距離をｒ１０、ｒ１１として図５（ｃ）に示した。
ここで、各アーム６〜１１において、前述の重心Ｐｇを回転中心とするイナーシャＪｃは、求めたい回転軸回りのイナーシャに変換する必要があるが、これは、慣性モーメントテンソルと回転行列を用いて求めたい回転軸回りのイナーシャに変換する。

一方、ロボット２の作業動作は、ティーチングペンダント４により教示され動作プログラムとしてＲＡＭ２０に記憶されている。ティーチングペンダント４によるロボット２の作業動作の教示は、アーム先端であるフランジ１１（フランジ１１の座標の原点）が通過する点を間欠的に指定することによって行われる。そして、ＣＰＵ１５は、実際にロボット２の作業動作を制御する場合、教示された通過点の間を補間してフランジ１１が滑らかな円弧を描いて通過点を通るように各アーム６〜１１のモータ１３を制御する。

この制御時、ＣＰＵ１５は、動作開始から一定の短時間が経過する毎にサンプリングタイムを迎え、予め設定された速度パターンに従って各サンプリングタイム時点でのフランジ１１の速度を決定し、その速度を積分することによってフランジ１１の移動距離を演算し、そして、動作プログラムからフランジ１１の移動軌跡を求め、各サンプリングタイムにおける移動軌跡上のフランジ１１の位置（サンプリング位置）と速度（速度ベクトル）を求める。フランジ１１のサンプリング位置と速度を求めると、ＣＰＵ１５は、座標変換によってフランジ１１以外の各アーム６〜１０の基準座標上でのサンプリング位置と速度とを求める。ＣＰＵ１５は、サンプリングタイム毎に、フランジ１１および他のアーム６〜１０が上記のようにして求めた位置と速度を取るように各モータ１３を制御し、これによってフランジ１１が補間された移動軌跡上を移動するようになるものである。

次に、モータ１３のトルクの伝達経路の伝達関数を求めるために、アームのイナーシャを求める手順を図３のフローチャートをも参照しながら説明する。このトルク伝達経路の伝達関数は、ロボット２に実際の作業動作を行わせる前に求めておくものである。図３に示すように、まず、ユーザは、ロボット２が停止していることを確認した上で（図３のステップＳ１）、ティーチングペンダント４の操作部４ａを操作し、ロボット制御装置３にアームイナーシャを求める動作を開始させる。すると、逆システム演算回路１８の演算部として機能するＣＰＵ１５は、動作プログラムから、一連の動作の開始点から終了点までの移動軌跡を取得し、実際の動作時におけると同様にしてサンプリングタイム毎の各アーム６〜１１のロボット座標上でのサンプリング位置および速度ベクトルを求めてＲＡＭ２０に記憶させる（ステップＳ２）。このとき、サンプル数はＮであったとする。

次に、ＣＰＵ１５は、カウンタを初期化し（ステップＳ３）、続いて当該カウンタをインクリメントする（ステップＳ４）。そして、カウンタのカウント数ｎに示された第ｎ回目のサンプリングタイムにおける各アーム６〜１１の速度ベクトルと、次の第（ｎ＋１）回目のサンプリングタイムにおける各アーム６〜１１の速度ベクトルをＲＡＭ２０から取得し、第ｎ回目のサンプリングタイムにおける各アーム６〜１１の速度ベクトルと第（ｎ＋１）回目のサンプリングタイムにおける各アーム６〜１１の速度ベクトルのなす角度θ＿ｎを求める（ステップＳ５：速度ベクトル取得手段、角度演算手段）。

この２つの速度ベクトルのなす角度θ＿ｎを求める方法の一つを例示すると、２つの速度ベクトルの外積および内積からｓｉｎθ＿ｎおよびｃｏｓθ＿ｎを求め（下記（２）式、（３）式）、これらｓｉｎθ＿ｎおよびｃｏｓθ＿ｎをＡｔａｎ２関数（座標から逆正接であるアークタンジェントを求める関数）のｘ座標値およびｙ座標値に代入し、θ＿ｎ（ラジアン）を求める（下記（４）式）、というものである。このようにＡｔａｎ２関数を使用してθを求めた場合には、内積或は外積だけからθを求める場合に比べ、θが０°または９０°に近い角度のときに、僅かなθの変化によるｓｉｎθまたはｃｏｓθの値の変化が少な過ぎまたは大き過ぎて、求めたθの誤差が大きくなるといった不具合を解消することができる。

θ＿ｎ＝Ａｔａｎ２（ｓｉｎθ＿ｎ，ｃｏｓθ＿ｎ）……（４）
θ＿ｎを求めると、次に、ＣＰＵ１５は、θ＿ｎが予め設定された値ε以上であるか否かを判断する（ステップＳ６:指定手段）。θ＿ｎがε以上である場合、ＣＰＵ１５は、各回転関節の軸を回転中心とするアームのイナーシャＪａ＿ｎおよび第１回目のサンプリングタイムのサンプリング位置と第２回目のサンプリングタイムのサンプリング位置をＲＡＭ２０から取得し、これら２位置間の距離ｄ＿ｎを演算し、これらイナーシャＪａ＿ｎおよびｄ＿ｎを記憶手段としてのＲＡＭ２０に記憶する。

続いて、ＣＰＵ１５は、カウンタをインクリメントし、上述したと同様にして、今回のサンプリングタイムと次回のサンプリングタイムの２つの速度ベクトルのなす角度θがε以上である場合、各回転関節の軸Ｌｃ−１〜Ｌｃ−６を中心とするアームのイナーシャＪａ＿ｎと今回のサンプリングタイムのサンプリング位置ｎと次回のサンプリングタイムのサンプリング位置（ｎ＋１）の距離ｄ＿ｎを演算してＲＡＭ１８に記憶する（ステップＳ７；イナーシャ取得手段、移動距離取得手段）、という動作をｎがＮとなるまで繰り返す。

つまり、フランジ１１のサンプリング位置が図４に黒点にて示すものであって、速度ベクトルＶｎとＶｎ＋１、Ｖｎ＋１とＶｎ＋２、Ｖｎ＋２とＶｎ＋３がε以上であったとすると、その間のサンプリング位置ｎ〜ｎ＋３のイナーシャＪａ＿ｎ、Ｊａ＿ｎ＋１、Ｊａ＿ｎ＋２と移動距離ｄ＿ｎ、ｄ＿ｎ＋１、ｄ＿ｎ＋２とを取得して記憶するのである。

カウンタのカウント数がＮになる（ステップＳ８で「ＹＥＳ」）と、ＣＰＵ１５は、ＲＡＭ２０に記憶したＪａ＿ｎからＪａ＿ａｖｅを算出する（ステップＳ９）。このＪａ＿ａｖｅの算出は、次の（５）式による。つまり、Ｊａ＿ｎに移動距離ｄ＿ｎを乗じてイナーシャ・距離積を求め（イナーシャ・距離積取得手段）、このイナーシャ・距離積の総和を、イナーシャ・距離積を求めたサンプリング位置間におけるフランジ１１の移動距離の総和で除して平均イナーシャ値Ｊａ＿ａｖｅを求める（平均イナーシャ取得手段）ものである。

以上のようにして求めたＪａ＿ａｖｅを用いて前述のモータ１３のトルク伝達経路の伝達関数を求め、サーボ駆動系を含むロボット全体の伝達関数Ｇ（ｓ）を求める。そして、伝達関数Ｇ（ｓ）の逆数１／Ｇ（ｓ）を逆システム演算回路１８の演算式とする。

ここで、Ｇ（ｓ）を求める場合に用いるアームのイナーシャＪａ−ａｖｅは、上述のように、隣り合う２つのサンプリング位置における２つの速度ベクトルのなす角度がε以上である円弧軌跡範囲を対象にして求める。この隣り合う２つのサンプリング位置における２つの速度ベクトルのなす角度がε以上であることは、フランジ１１の移動軌跡が所定の曲率半径以下の円弧状軌跡であることを意味する。そして、このときのイナーシャＪａ＿ｎに移動距離を掛け、その積の総和を、所定の曲率半径以下の円弧状軌跡で移動した距離の総和で除した値Ｊａ＿ａｖｅは、フランジ１１が所定の曲率半径以下の円弧状軌跡を移動するときのイナーシャの単なる平均であることのみならず、これに重み付けを行っていることを意味する。この場合、イナーシャにサンプリングタイム間の距離を掛けた積は、アームの「仕事」の意味を持つので、アームがサンプリング位置でそのまま速度ベクトルの示す方向に移動するエネルギーの大きさの意味を持っている。

このようなＪａ＿ａｖｅを用いて求めた伝達関数Ｇ（ｓ）を求め、この伝達関数Ｇ（ｓ）の逆数１／Ｇ（ｓ）を逆システム演算回路１８の演算式とする本実施形態によれば、ユーザがティーチングペンダント１７を用いた教示した通過点をより正確に通る移動軌跡が得られる。そして、アーム先端が動作遅れを起こしやすい曲線の動作軌跡部分、中でも所定の曲率半径以下の曲線の動作軌跡部分におけるアームのイナーシャを用いてＧ（ｓ）を求めているので、直線の動作軌跡部分は勿論のこと、曲率半径の小さな動作軌跡部分でも、動作遅れを極力防止しながらアーム６〜１０の動作を制御でき、高い軌跡精度の制御を行うことが可能となる。

なお、本発明は上記し且つ図面に示す実施形態に限定されるものではなく、以下のような拡張或は変更が可能である。
Ｊａ−ａｖｅを求める場合に用いるフランジ１１の移動距離は、各サンプリング位置の１つ前のサンプリング位置から各サンプリング位置までの移動距離を用いるようにしても良い。このようにしても、フランジ１１の移動距離は、実質的に、各サンプリング位置から次のサンプリング位置までの移動距離を求めたこととなる。
Ｊａ−ａｖｅを求める場合の円弧状動作軌跡部分は、上述の実施形態のようにＣＰＵ１５が自動的に設定するものに限られず、ユーザが指定するようにしても良い。つまり、動作軌跡をティーチングペンダント４の表示器４ｂに表示し、この表示された動作軌跡から曲率半径の小さい円弧動作軌跡部分を指定して当該指定された円弧動作軌跡を対象にＪａ−ａｖｅを求めるようにしても良い。
フランジ１１に取り付けられるエンドエフェクタをイナーシャを回転関節の軸を中心とするアームのイナーシャに加えるようにしても良い。
ロボットとしては、垂直多関節型のものに限られない。

図面中、１は産業用ロボット、２はロボット、３はロボット制御装置、４はティーチングペンダント、５はベース、６〜１１はアーム、１３はモータ、１４は減速装置、１５はＣＰＵ（制御手段、イナーシャ取得手段、移動距離取得手段、イナーシャ・距離積取得手段、平均イナーシャ取得手段、速度ベクトル取得手段、角度演算手段、指定手段）、１６は駆動回路、１７は位置検出回路、１８は逆システム演算回路（逆システム演算手段）、２１はロータリエンコーダを示す。

Claims (4)

1. 複数のアームを順次関節により連結して構成されたロボットを制御対象とし、
   前記複数の各アームの目標位置が入力されると、前記各アームについて、所定の演算式により前記目標位置を修正した修正目標位置を演算する逆システム演算手段と、前記逆システム演算手段から前記修正目標位置が与えられると、前記各アームを駆動するアクチュエータを制御して前記各アームが前記修正目標位置に移動するように制御する制御手段とを備えたロボット制御装置において、
   前記逆システム演算手段は、前記各アームの前記修正目標位置を演算するための前記各アームについての前記演算式中に、当該アームの前記関節を回転中心とするイナーシャをパラメータとして有し、
   前記各アームについての前記演算式中の前記イナーシャは、エンドエフェクタを取り付けるためのアーム先端が、動作プログラムにより定められる移動軌跡に沿って、同じく前記動作プログラムにより定められる速度で移動するとしたとき、前記アーム先端の前記移動軌跡のうち、特定の弧状軌跡を対象にして、所定の時間間隔をもってサンプリングされるサンプリング位置毎に、前記関節を回転中心とする前記アームのイナーシャであって、当該関節よりも前記アーム先端側に存在する全てのアームのイナーシャを取得し、この取得した各サンプリング位置での前記イナーシャに、当該サンプリング位置から次のサンプリング位置までの前記アーム先端の移動距離を乗じてイナーシャ・距離積を求め、そのイナーシャ・距離積の総和を、前記イナーシャ・距離積を求めた前記サンプリング位置相互間における前記アーム先端の移動距離の総和で除して平均イナーシャ値を求め、この平均イナーシャ値を前記修正目標位置を計算するための前記演算式のイナーシャとして用いることによりイナーシャ取得を行うことを特徴とするロボット制御装置。
   
2. 前記特定の弧状軌跡を自動指定するために、前記アーム先端が動作プログラムにより定められる軌跡を、同じく前記動作プログラムにより定められる速度で移動するとしたとき、隣どうしのサンプリング位置における２つの速度ベクトルの外積および内積からそれら２つの速度ベクトルのなす角度θの正弦値ｓｉｎθおよび余弦値ｃｏｓθを求め、それら正弦値ｓｉｎθおよび余弦値ｃｏｓθをＡｔａｎ２関数のｘ座標値およびｙ座標値に代入してθを求め、このθが予め定められた所定角度以上である範囲を、前記弧状軌跡に定めることを特徴とする請求項１記載のロボット制御装置。
3. 複数のアームを順次関節により連結して構成されたロボットを制御対象とし、
   前記複数の各アームの目標位置が入力されると、前記各アームについて、所定の演算式により前記目標位置を修正した修正目標位置を演算する逆システム演算手段と、前記逆システム演算手段から前記修正目標位置が与えられると、前記各アームを駆動するアクチュエータを制御して前記各アームが前記修正目標位置に移動するように制御する制御手段とを備えたロボット制御装置において、
   前記逆システム演算手段は、前記各アームの前記修正目標位置を演算するための前記各アームについての前記演算式中に、当該アームの前記関節を回転中心とするイナーシャをパラメータとして有し、
   前記各アームの前記関節を回転中心とするイナーシャを取得するために、
   エンドエフェクタを取り付けるためのアーム先端が、動作プログラムにより定められる移動軌跡に沿って、同じく前記動作プログラムにより定められる速度で移動するとしたとき、前記アーム先端の前記移動軌跡のうち、特定の弧状軌跡を対象にして、所定の時間間隔をもってサンプリングされるサンプリング位置毎に、前記関節を回転中心とする前記アームのイナーシャであって、当該関節よりも前記アーム先端側に存在する全てのアームのイナーシャを取得するイナーシャ取得手段と、
   前記各サンプリング位置から次のサンプリング位置までの前記アーム先端の移動距離を求める移動距離取得手段と、
   前記各サンプリング位置での前記イナーシャに、当該サンプリング位置から次のサンプリング位置までの前記アーム先端の移動距離を乗じてイナーシャ・距離積を求めるイナーシャ・距離積取得手段と、
   前記イナーシャ・距離積の総和を、前記イナーシャ・距離積を求めた前記サンプリング位置相互間における前記アーム先端の移動距離の総和で除して平均イナーシャ値を求める平均イナーシャ取得手段と、を備え、
   前記逆システム演算手段は、前記平均イナーシャ値を、前記修正目標位置を計算するための前記演算式のイナーシャとして用いることを特徴とするロボット制御装置。
4. 更に、前記特定の弧状軌跡を自動指定するために、前記アーム先端が動作プログラムにより定められる軌跡を、同じく前記動作プログラムにより定められる速度で移動するとしたとき、隣どうしのサンプリング位置における２つの速度ベクトルを取得する速度ベクトル取得手段と、
   前記速度ベクトル取得手段が取得した前記２つの速度ベクトルの外積および内積からそれら２つの速度ベクトルのなす角度θの正弦値ｓｉｎθおよび余弦値ｃｏｓθを求め、それら正弦値ｓｉｎθおよび余弦値ｃｏｓθをＡｔａｎ２関数のｘ座標値およびｙ座標値に代入してθを求める角度演算手段と、
   前記角度演算手段により求められた前記θが予め定められた所定角度以上である範囲を、前記弧状軌跡に定める指定手段と、
   を備えることを特徴とする請求項３記載のロボット制御装置。

Images