JP2009113172A - 指令値生成装置 - Google Patents

Abstract

【課題】特異点近傍を通過する際、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制して、動作時間を短縮できる指令値生成装置を得る。
【解決手段】予め領域を設定された特異点近傍領域に入る、もしくは通過したことを判別する特異点近傍領域突入判別手段８と、特異点近傍領域突入判別手段８で特異点近傍領域に入る、もしくは通過したと判別した場合に当該動作におけるロボット先端軌跡上での特異点近傍領域始点と特異点近傍領域終点を算出する近傍領域算出手段９を備え、ロボット先端軌跡上の特異点近傍領域始点と特異点近傍領域終点の間の区間では、当該始点及び終点の関節変位に基づいて関節位置指令を算出する。
【選択図】図１

Description

この発明は、ロボット、工作機械、レーザ加工機等、複数の関節を備えたメカニカルシステムの先端を、各関節の駆動軸を制御することにより、予め定められた軌跡に沿って移動させる際の各関節の駆動軸を制御するための指令値を生成する指令値生成装置に関する。

公知の指令値生成装置では、移動速度・移動量演算手段で算出された加減速前の先端位置指令に対して逆変換を行った結果、指定された特異点近傍エリア内にあり、かつ、その周期の関節の移動量が閾値以上である場合は、その特異点に関して逆変換を行う場合の変換関数を決めるフラグを変更して再度逆変換を行い、できるだけ短時間で変更したフラグでの逆変換に移行するとともに、フラグを切替ている区間を記憶しておき、加減速後の直線上の位置指令を逆変換する際には、切替区間に到達する前は切換え区間に到達するまでのフラグで逆変換を行い、切換え区間通過後は切替区間通過後のフラグで逆変換を行い、切替区間では区間の両端の関節変位に基づいて逆変換を実施している（例えば、特許文献１参照）。

特開２００６−１１６６５８号公報

しかしながら、上述した指令値生成装置では、特異点近傍を通過する際、複数の関節が大きく動き、すべての軸で速度が許容値以下となるためには先端速度を大きく落とす必要がある場合があった。

この発明は上述した点に鑑みてなされたもので、特異点近傍を通過する際、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制して、動作時間を短縮できる指令値生成装置を得ることを目的とする。

この発明に係る指令値生成装置は、複数の関節を備えたメカニカルシステムの先端を、各関節の駆動軸を制御することにより、予め定められた軌跡に沿って移動させる際の各関節の駆動軸を制御するための指令値を生成する指令値生成装置であって、加減速前のメカニカルシステム先端位置指令を演算する移動速度・移動量演算手段と、前記移動速度・移動量演算手段で演算する加減速前のメカニカルシステム先端位置指令を各関節の位置指令に変換する第１の逆変換手段と、加減速後のメカニカルシステム先端位置指令を各関節の位置指令に変換する第２の逆変換手段と、前記第１の逆変換手段からの関節変位に基づいて予め指定される特異点近傍領域にメカニカルシステムの先端位置指令が入るもしくは通過したことを判別する特異点近傍領域突入判別手段と、前記特異点近傍領域突入判別手段で特異点近傍領域に入るもしくは通過したと判別した場合に、当該動作におけるメカニカルシステム先端軌跡上での特異点近傍領域の始点と終点を算出する近傍領域算出手段と、前記近傍領域算出手段により算出した始点及び終点を記憶する近傍領域記憶手段とを備え、前記第１と第２の逆変換手段は、前記近傍領域記憶手段に記憶されるデータに基づいてメカニカルシステム先端軌跡上の特異点近傍領域の始点と終点の間の区間では、当該始点及び終点の関節変位に基づいて各関節の位置指令を算出することを特徴とする。

この発明によれば、メカニカルシステム先端軌跡上の特異点近傍領域の始点と終点の間の区間では、当該始点及び終点の関節変位に基づいて各関節の位置指令を算出するようにしたので、特異点近傍を通過する際、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制できるため、動作時間を短縮できる効果がある。

実施の形態１．
図１は、この発明の実施の形態１による指令値生成装置を示すブロック図である。この発明は、ロボット、工作機械などのメカニカルシステムの先端を指定された軌道に沿って動かす場合のメカニカルシステムの各軸の位置指令または速度指令の生成装置に関するものであり、以下、実施の形態１における指令値生成装置の概要をまず説明し、後段で、各構成要素毎にその内容の詳細を説明する。

移動速度・移動量演算手段１では、ロボット、工作機械などのメカニカルシステムの先端を指定された軌道に沿って動かす場合の軌道の各地点毎（移動量毎）にメカニカルシステムの各軸（各関節）の速度制限を満たし、かつ規定の速度以下となる移動速度を算出するもので、メカニカルシステムの先端の移動速度を演算すると共に、各関節変位から各関節速度を演算し、各関節速度と当該各関節で定められた所定の最大許容値との比に応じて先端の移動速度を修正して出力する移動速度演算手段５と、移動速度演算手段５からの移動速度に基づいて加減速前の先端位置指令である移動量を演算する移動量演算手段６と、後述する近傍領域記憶手段１０から送信情報に基づいて移動量演算手段６が演算した加減速前の先端位置指令を逆変換して各関節変位を演算する第１の逆変換手段としての逆変換手段７とを有する。

移動速度修正手段２では、移動速度・移動量演算手段１で算出した移動速度に対して加減速を考慮した修正を加え、制御周期毎の加減速前の移動速度を算出する。加減速手段３では、移動速度修正手段２で修正した制御周期毎の移動速度に対して２段の移動平均フィルタ処理を行い、制御周期ごとのメカニカルシステムの先端位置指令を生成する。

第２の逆変換手段としての逆変換手段４では、後述する近傍領域記憶手段１０から送信情報に基づいて加減速手段３から出力されるメカニカルシステムの加減速後の先端位置指令を各軸の位置指令に変換する逆変換を実施して各軸指令を出力する。

次に、各部の詳細を説明する。移動速度・移動量演算手段１では、その内部の移動速度演算手段５で移動速度の規定値ｖ_０を設定する。移動速度の規定値はメカニカルシステムに固有の先端最高速度とする。移動速度演算手段５および移動量演算手段６は、両者の演算回数の累積が加減速手段３で備える移動平均フィルタの窓長の総和を超えるかどうかで動作が異なる。即ち、移動量演算手段６で移動量を算出する際に用いる時間刻みをｓｔ秒、移動平均フィルタの窓長の総和をｆｔ秒とするとき、移動速度演算手段５および移動量演算手段６の演算回数の累積ｋの２倍がｆｔ／ｓｔを超えるかどうかで移動速度演算手段５および移動量演算手段６の動作が異なる。

以下、図２及び図３のフローチャートを適宜参照して移動速度・移動量演算手段１の動作を説明する。演算ステップの累積回数をｋ（ｋ＝１，２，３，・・・）としたとき、２×ｋ≦ｆｔ／ｓｔを満たす最大のｋをｋ１とする。この実施の形態１では、動作開始（ｋ＝１）から２×ｋ≦ｆｔ／ｓｔ、即ち、ｋ≦ｋ１の間は（ステップＳ１でＹＥＳ）、２周期分、即ち、２回分の計算を１度に行う。そして、動作開始時には移動速度の初期値を規定値の２倍である２×ｖ_０とし、ｋ回目の周期における移動速度の初期値を２×ｖ_ｋ−１とする（ステップＳ２）。ここで、ｋ＝１のとき、ｖ_ｋ−１は規定値ｖ_０とする。すなわち、移動速度演算手段５では、移動速度初期値を２×ｖ_ｋ−１に設定し、移動量演算手段６に出力する。

これらの処理は、動作開始からの移動速度・移動量演算手段１での制御の立ち上がりを高めるため、速度指令として実際の値より大きい値、ここでは、２倍に設定し、２回分の計算を１度で行うようにしている。制御をはやめる方式として、倍率を変えたり、もっと広い範囲に適用することは可能であり、始点から終点までの全てに適用することも理論的には可能である。この例で、移動平均フィルタの１／２の範囲にとどめているのは必要最小限に留めるためである。必要最小限に留めたいのは、速度が高いほど先端の動きと各軸の動きとの線形性が崩れ、各軸の速度が上限を超えない範囲で先端速度を定めていても、線形性が崩れると実際には上限を超えてしまう場合が生じうる。線形性の強さは速度が低いほど強くなるため、この実施の形態１では、速度を２倍で評価するのは必要最小限に留めている。

次に、移動量算出手段６で、ｓｔ秒での移動量ｄｌ_ｋを下式（１）に従って算出し、
ｄｌ_ｋ＝２×ｖ_ｋ−１×ｓｔ （１）
さらに、移動量ｄｌ_ｋの累積値ｌ_ｋを算出し、動作開始地点からｌ_ｋだけ進んだ地点ｐ_ｋを算出する。

次に、逆変換手段７で、地点ｐ_ｋを逆変換し、逆変換結果ｔｈ_ｋを得る。なお、逆変換手段７の動作については後ほど詳細を説明する。逆変換手段７による逆変換結果ｔｈ_ｋは、移動速度演算手段５にフィードバックされ、移動速度演算手段５で、前回の演算における逆変換結果ｔｈ_ｋ−１から各軸毎の移動量ｄｔｈ_ｋの計算が下式（２）に従って実施される。
ｄｔｈ_ｋ＝ｔｈ_ｋ−ｔｈ_ｋ−１ （２）

さらに、移動速度演算手段５は、移動量ｄｔｈ_ｋの各軸成分とそれぞれの許容最大値ｄｔｈｍａｘの２倍との比を下式（３）に従って算出（ｉはメカニカルシステムの第ｉ軸を意味する）し、ｒｄｔｈ_ｋ（ｉ）の最大値ｒｄｔｈ_ｋｍａｘを求める（ステップＳ３）。
ｒｄｔｈ_ｋ（ｉ）＝|ｄｔｈ_ｋ（ｉ）|／（２×ｄｔｈｍａｘ（ｉ）） （３）

ここで、ｒｄｔｈ_ｋｍａｘ≦１の場合（メカニカルシステムの軸数をｎとし、１≦ｉ≦ｎを満たすすべての自然数ｉに対して|ｄｔｈ_ｋ（ｉ）|≦２×ｄｔｈｍａｘ（ｉ）が成り立つ場合、ステップＳ４でＹＥＳ）は移動速度、移動量として１回目の計算結果をそのまま採用し、２回分を１回で行った計算結果を本来のステップでの値に換算する処理を行う。

即ち、２×ｋ−１番目の移動速度ｖｓ（２×ｋ−１）および２×ｋ番目の移動速度ｖｓ（２×ｋ）をいずれもｖ_ｋ−１とする（ステップＳ５）。
また、２×ｋ−１番目の移動量ｌｓ（２×ｋ−１）を、
ｌｓ（２×ｋ−１）＝ｌ_ｋ−１＋０．５×ｄｌ_ｋ （４）
２×ｋ番目の移動量を、
ｌｓ（２×ｋ）＝ｌ_ｋ （５）
とする。

また、ｖ_ｋ−１＝ｖ_０の場合は
ｖ_ｋ＝ｖ_０ （６）
とする。
また、ｖ_ｋ−１＜ｖ_０の場合は
ｖ_ｋ＝ｍｉｎ（ｖ_ｋ−１／ｒｄｔｈ_ｋｍａｘ，ｖ_０） （７）
とする（ステップＳ６）。ここで、ｍｉｎ（）は最小値を意味している。

式（７）から分かるように、各軸の速度に余裕があっても移動速度の規定値ｖ_０以上には速度を上げない。上限を移動速度の規定値ｖ_０と設定しておくことにより、移動速度の規定値ｖ_０で各軸速度が許容値を超えない場合は再度計算する必要がなく、計算量が削減する。仮に、少なくとも１つの軸の速度が許容値に達するように動作させる場合は、毎周期再度の計算が必要となり計算量が増大する。

ｒｄｔｈ_ｋｍａｘ＞１の場合（ステップＳ４でＮＯ）は、いずれかの軸の速度が許容最大値を超えているので、移動速度を２×ｖ_ｋ−１／ｒｄｔｈ_ｋｍａｘに修正し、移動量ｄｌ_ｋ及び累積値ｌ_ｋを算出し直す。そして、２×ｋ−１番目の移動速度ｖｓ（２×ｋ−１）および２×ｋ番目の移動速度ｖｓ（２×ｋ）をいずれもｖ_ｋ−１／ｒｄｔｈ_ｋｍａｘとする（ステップＳ７）。また、移動量に関しては求め直した移動量ｄｌ_ｋ及び累積値ｌ_ｋを用いて、２×ｋ−１番目の移動量ｌｓ（２×ｋ−１）を、
ｌｓ（２×ｋ−１）＝ｌ_ｋ-1−１＋０．５×ｄｌ_ｋ （８）
また、２×ｋ番目の移動量を、
ｌｓ（２×ｋ）＝ｌ_ｋ （９）
とする。また、
ｖ_ｋ＝ｖ_ｋ−１／ｒｄｔｈ_ｋｍａｘ （１０）
とする（ステップＳ８）。

２×ｋ＞ｆｔ／ｓｔとなって以降、即ち、ｋ＞ｋ１になると（ステップＳ１でＮＯで、以下、図３のフローに移る）、今まで、２回分の計算を１度に実施していたのを、１度の計算で１回分を計算する処理に切り替える。ｋ＞ｋ１では、第ｋ番目の周期の演算をｋ２＝ｋ＋ｋｌ番目の周期の演算とみなすことにし（ステップＳ９）、ｖ_２×ｋｌ＝ｖ_ｋｌ、ｌ_２×ｋｌ＝ｌ_ｋｌとする（ステップＳ１０）。

即ち、第ｋｌ周期（ここまでは２回分計算）では、２×ｋｌ−１番目のデータと２×ｋｌ番目のデータを計算している。第ｋｌ＋１回目の周期（ここからは１回分計算）では１回分の計算をすることになるが、本当はｋｌ＋１回目の周期に相当するので、２×ｋｌ回目の周期まで１度に１回分の計算を行ってきたとみなしている。つまり、２×ｋｌ＋１番目の周期であるとして計算する。

同様に、第ｋｌ＋２番目の周期では、２×ｋｌ＋２番目の周期であるとして計算する。これを一般的な表現で書けば、ｋ＋ｋｌ番目の周期とみなすことになる。また、本当はｋｌ＋１回目の周期であるのを２×ｋｌ＋１番目の周期であるとして計算する場合、前回の速度はｖ２×ｋｌ、前回の移動量はｌ２×ｋｌとなるが、それらの値はそれぞれｖｓ（２×ｋｌ）、ｌｓ（２×ｋ）であるため、ｖ_２×ｋｌ＝ｖ_ｋｌ、ｌ_２×ｋｌ＝ｌ_ｋｌとなる。

次に、移動量算出手段６で、ｓｔ秒での移動量ｄｌ_ｋ２＝ｖ_ｋ２−１×ｓｔを算出し、さらに、移動量ｄｌ_ｋ２の累積値ｌ_ｋ２を算出し動作開始地点からｌ_ｋ２だけ進んだ地点ｐ_ｋ２を算出する。

次に、逆変換手段７で、地点ｐ_ｋ２を逆変換し、逆変換結果ｔｈ_ｋ２を得る。なお、逆変換手段７の動作については後ほど詳細を説明する。逆変換結果ｔｈ_ｋ２は、移動速度演算手段５にフィードバックされ、移動速度演算手段５で、前回の演算における逆変換結果ｔｈ_ｋ２−１から各軸毎の移動量ｄｔｈ_ｋ２＝ｔｈ_ｋ２−ｔｈ_ｋ２−１の計算を実施する。さらに、移動量ｄｔｈ_ｋ２の各軸成分とそれぞれの許容最大値ｄｔｈｍａｘとの比を下式（１１）に従って算出（ｉはメカニカルシステムの第ｉ軸を意味する）し（ステップＳ１１）、ｒｄｔｈ_ｋ２（ｉ）の最大値ｒｄｔｈ_ｋ２ｍａｘを求める。
ｒｄｔｈ_ｋ２（ｉ）＝|ｄｔｈ_ｋ２（ｉ）|／（ｄｔｈｍａｘ（ｉ）） （１１）

ここで、ｒｄｔｈ_ｋ２ｍａｘ≦１の場合は（ステップＳ１２でＹＥＳ）、移動速度、移動量として１回目の計算結果をそのまま採用し、ｋ２番目の移動速度ｖｓ（ｋ２）をｖ_ｋ２−１とする（ステップＳ１３）。また、ｋ２番目の移動量ｌｓ（ｋ２）をｌｓ（ｋ２）＝ｌ_ｋ２とする。
また、ｖ_ｋ２−１＝ｖ_０の場合は
ｖ_ｋ２＝ｖ_０ （１２）
とする。ｖ_ｋ２−１＜ｖ_０の場合は
ｖ_ｋ２＝ｍｉｎ（ｖ_ｋ２−１／ｒｄｔｈ_ｋ２ｍａｘ，ｖ_０） （１３）
とする（ステップＳ１４）。ここで、ｍｉｎ（）は最小値を意味している。

ｒｄｔｈ_ｋ２ｍａｘ＞１の場合（ステップＳ１２でＮＯ）は、移動速度をｖ_ｋ２−１／ｒｄｔｈ_ｋ２ｍａｘに修正し、移動量ｄｌ_ｋ２及び累積値ｌ_ｋ２を算出し直す。ｋ２番目の移動速度ｖｓ（ｋ２）をｖ_ｋ２−１／ｒｄｔｈ_ｋ２ｍａｘとする（ステップＳ１５）。また、移動量に関しては求め直した移動量ｄｌ_ｋ２及び累積値ｌ_ｋ２を用いて、ｌｓ（ｋ２）＝ｌ_ｋ２とする。また、ｖ_ｋ２＝ｖ_ｋ２−１／ｒｄｔｈ_ｋ２ｍａｘとする（ステップＳ１６）。

次に、逆変換手段４、逆変換手段７での動作について説明する。逆変換手段４、７で先端位置から各軸の位置を求める逆変換を実施する。一般に、逆変換の解は複数存在し、どの解を算出するかを指定する必要がある。ここでは、この解の指定のための変数をフラグと呼ぶことにする。例えば対象とするメカニカルシステムが図４に示す垂直６軸（Ｊ１軸〜Ｊ６軸）ロボットの場合、図５〜図７に示す３組のフラグが存在する。

逆変換手段７では、移動量演算手段６の出力が後述する特異点近傍領域の始点に到達するまでは動作開始地点で指定されているフラグを用いて逆変換する。図４の垂直６軸ロボットでは、図５のＲｉｇｈｔ／Ｌｅｆｔ、図６のＡｂｏｖｅ／Ｂｅｌｏｗ、図７のＦｌｉｐ／ＮｏｎＦｌｉｐからそれぞれ１つ指定される（例えば、Ｒｉｇｈｔ，Ａｂｏｖｅ，Ｆｌｉｐ）。移動量演算手段６の出力が後述する特異点近傍領域にある場合は、特異点近傍領域の始点及び終点の各軸位置に基づいて逆変換を行う。特異点近傍領域にある場合の逆変換方法については後述する。さらに、移動量演算手段６の出力が後述する特異点近傍領域の終点を通過後は、特異点に応じて切り替えたフラグで逆変換を行う。

逆変換手段４も、逆変換手段７と同様に、加減速手段３の出力が後述する特異点近傍領域の始点に到達するまでは動作開始地点で指定されているフラグを用いて逆変換する。加減速手段３の出力が後述する特異点近傍領域にある場合は、特異点近傍領域の始点及び終点の各軸位置に基づいて逆変換を行う。特異点近傍領域にある場合の逆変換方法については後述する。さらに、加減速手段３の出力が後述する特異点近傍領域の終点を通過後は、特異点に応じて切り替えたフラグで逆変換を行う。

次に、特異点近傍領域突入判別手段８、近傍領域算出手段９、近傍領域記憶手段１０および特異点に応じたフラグの切り替えについて説明する。まず、近傍領域の範囲を予め指定しておく。Ｆｌｉｐ／ＮｏｎＦｌｉｐ（以後、Ｆ／ＮＦと略称する）ではＪ５軸の角度（０度からの角度）、Ａｂｏｖｅ／Ｂｅｌｏｗ（以後、Ａ／Ｂと略称する）ではＪ３軸の角度、Ｒｉｇｈｔ／Ｌｅｆｔ（以後、Ｒ／Ｌと略称する）ではＪ１軸回転軸からＪ５軸までの距離で領域の範囲を指定する。例えば、Ｆ／ＮＦならＪ５軸の角度として±５度の範囲を特異点近傍領域とする。

特異点近傍領域突入判別手段８により近傍領域に初めて入った場合、もしくは近傍領域を跳び越したと判定した場合に、近傍領域算出手段９で近傍領域の始点・終点を算出する。近傍領域に入ったかどうかは、Ｆ／ＮＦ及びＡ／Ｂに関しては逆変換手段７より出力される関節角度から判別できる。Ｒ／Ｌに関しては逆変換手段７の出力からＪ１軸回転軸からＪ５軸までの距離を算出すれば判別できる。例えば逆変換手段７より出力されるＪ５軸の角度が±５度以内に入った場合、Ｆ／ＮＦを切り替える特異点近傍領域に入ったと判別する。

また、今回の逆変換手段７の出力と前回の逆変換手段７の出力から、関節角の１周期での移動量が規定値を超えると判定された場合に、特異点近傍領域突入判別手段８は、近傍領域を跳び越したと判別する。例えばＪ４軸、Ｊ６軸の関節角の１周期での移動量が規定値を超えると判定された場合に、Ｆ／ＮＦを切り替える特異点近傍領域を跳び越したと判別する。

次に、特異点近傍領域の始点及び終点の算出方法をＦ／ＮＦを切り替える特異点の場合を例に挙げて説明する。Ｆ／ＮＦで指定されている近傍範囲を±ｄｊ５、近傍領域に入ったのが第ｋ回目の周期とし、第ｋ回目の周期での直線上の移動量（進行方向に進んでいるときに正の値）を△ｌ（ｋ）、第ｋ回目の周期での直線上の位置をｐ（ｋ）、第ｋ回目の周期のＪ５軸角度をｑ５（ｋ）、第ｋ−１回目の周期のＪ５軸角度をｑ５（ｋ−１）とする。

また、
（Ａ）Ｊ５軸が負の値から特異点に近づいてきて近傍領域に入った場合、
（Ｂ）Ｊ５軸が正の値から特異点に近づいてきて近傍領域に入った場合、
（Ｃ）Ｊ５軸が負の値から特異点に近づいてきて近傍領域を跳び越した場合、
（Ｄ）Ｊ５軸が正の値から特異点に近づいてきて近傍領域を跳び越した場合
の４通りに分けて考える。

（Ａ）Ｊ５軸が負の値から特異点に近づいてきて近傍領域に入った場合
まず、第ｋ回目の移動量演算手段の出力の地点でＦ／ＮＦに関するもう一方のフラグ（ＮｏｎＦｌｉｐ）で逆変換する。元のフラグで逆変換したときの第ｋ周期での各軸の移動量の最大値ともう一方のフラグで逆変換したときの各軸移動量の最大値を求め、最大値の小さい方のフラグを第ｋ回目の周期での先端軌跡上の地点でのフラグとする。また、Ｊ５軸の角度ｑ５（ｋ）も選択された方のフラグでの逆変換結果とする。

このとき、
ｌａ（１）＝（−ｄｊ５−ｑ５（ｋ））／
（ｑ５（ｋ）−ｑ５（ｋ−１））×△ｌ（ｋ） （１４）
でｌａ（１）を算出し、第ｋ回目の粗補間周期での直線上の位置ｐ（ｋ）からｌａ（１）だけ進んだ位置ｐａ（１）を近傍エリアの始点とする。また、
ｌｂ（１）＝（ｄｊ５−ｑ５（ｋ））／
（ｑ５（ｋ）−ｑ５（ｋ−１））×△ｌ（ｋ） （１５）
でｌｂ（１）を算出し、第ｋ回目の周期での移動量演算手段の出力である先端軌跡上の位置ｐ（ｋ）からｌｂ（１）だけ進んだ位置ｐｂ（１）を近傍領域の終点とする。

近傍領域の始点・終点は繰り返し計算により求めることとし、指定されている繰り返し回数ｎが１の場合は、ｐａ（１）、ｐｂ（１）をそのまま近傍領域の始点・終点とする。

次に、繰り返し回数ｎが２以上の場合、第ｉ回目の繰り返しでの近傍領域始点、終点をｐａ（ｉ）、ｐｂ（ｉ）、第ｉ回目の繰り返しでの始点及び終点の移動量をｌａ（ｉ）、ｌｂ（ｉ）、ｐａ（ｉ）を逆変換した時のＪ５軸角度をｑａ５（ｉ）、ｐｂ（ｉ）を逆変換した時のＪ５軸角度をｑｂ５（ｉ）とし、
ｌａ（ｉ＋１）＝（−ｄｊ５−ｑ５ａ（ｉ））／
（ｑ５ａ（ｉ）−ｑ５ａ（ｉ−１））×ｌａ（ｉ） （１６）
ｌｂ（ｉ＋１）＝（ｄｊ５−ｑ５ｂ（ｉ））／
（ｑ５ｂ（ｉ）−ｑ５ｂ（ｉ−１））×ｌｂ（ｉ） （１７）
で次の繰り返しでの近傍領域始点、終点までの距離を算出しｐａ（ｉ＋１）＝ｐａ（ｉ）＋ｌａ（ｉ＋１）、ｐｂ（ｉ＋１）＝ｐｂ（ｉ）＋ｌｂ（ｉ＋１）で次の繰り返しでの始点・終点を算出し、ｐａ（ｎ）、ｐｂ（ｎ）を近傍領域の始点・終点の位置とする。

ここで、ｑ５ａ（０）＝ｑ５（ｋ）、ｑ５ｂ（０）＝ｑ５（ｋ）とする。また、各繰り返し周期での逆変換結果ｑａ５（ｉ）、ｑｂ５（ｉ）を算出する際、どちらのフラグで逆変換したほうが第ｋ−１回目の粗補間での逆変換結果からの各軸移動距離の最大値が小さくなるかを判別し、小さい方のフラグで逆変換する。一度元のフラグの方が各軸移動距離の最大値が小さいことが判明した地点よりも第ｋ−１回目の周期の移動量演算手段の出力側の地点では元のフラグでのみ逆変換し、一度切替えたフラグの方が各軸移動距離の最大値が小さいことが判明した地点よりもさらに進んだ地点では切替えたフラグでのみ逆変換することにより、繰り返し計算での逆変換回数を削減する。

なお、繰り返し計算の途中で、ｑｂ５（ｉ）＜−ｄｊ５となることがあれば、一旦近傍領域に入ったがフラグを変えずに近傍領域外に出る場合であると判別し、その時点で繰り返し計算を終了し、近傍領域はなしとしてｐｂ５（ｉ）を通過するまでは元のフラグのまま直線補間を続ける。ｐｂ５（ｉ）を通過は近傍領域に再度入ることがあればその周期で再度近傍領域の算出を実行する。

（Ｂ）Ｊ５軸が正の値から特異点に近づいてきて近傍領域に入った場合
まず、第ｋ回目の移動量演算手段の出力の地点でＦ／ＮＦに関するもう一方のフラグ（Ｆｌｉｐ）で逆変換する。元のフラグで逆変換したときの第ｋ周期での各軸の移動量の最大値ともう一方のフラグで逆変換したときの各軸移動量の最大値を求め、最大値の小さい方のフラグを第ｋ回目の周期での直線上の地点でのフラグとする。また、Ｊ５軸の角度ｑ５（ｋ）も選択された方のフラグでの逆変換結果とする。

このとき、（１５）〜（１８）式の代わりに（１９）〜（２２）式でｌａ（ｉ）、ｌｂ（ｉ）を算出し、（Ａ）のＪ５軸が負の値から特異点に近づいてきて近傍領域に入った場合と同様に近傍領域の始点・終点の位置を算出する。
ｌａ（１）＝（ｄｊ５−ｑ５（ｋ））／
（ｑ５（ｋ）−ｑ５（ｋ−１））×△ｌ（ｋ） （１９）
ｌｂ（１）＝（−ｄｊ５−ｑ５（ｋ））／
（ｑ５（ｋ）−ｑ５（ｋ−１））×△ｌ（ｋ） （２０）
ｌａ（ｉ＋１）＝（ｄｊ５−ｑ５ａ（ｉ））／
（ｑ５ａ（ｉ）−ｑ５ａ（ｉ−１））×ｌａ（ｉ） （２１）
ｌｂ（ｉ＋１）＝（−ｄｊ５−ｑ５ｂ（ｉ））／
（ｑ５ｂ（ｉ）−ｑ５ｂ（ｉ−１））×ｌｂ（ｉ） （２２）

なお、繰り返し計算の途中で、ｑｂ５（ｉ）＞ｄｊ５となることがあれば、一旦近傍領域に入ったがフラグを変えずに近傍領域外に出る場合であると判別し、その時点で繰り返し計算を終了し、近傍領域はなしとしてｐｂ５（ｉ）を通過するまでは元のフラグのまま直線補間を続ける。ｐｂ５（ｉ）を通過後、近傍領域に再度入ることがあればその周期で再度近傍領域の算出を実行する。

（Ｃ）Ｊ５軸が負の値から特異点に近づいてきて近傍領域を跳び越した場合
まず、第ｋ回目の粗補間の地点でＦ／ＮＦに関するもう一方のフラグ（ＮｏｎＦｌｉｐ）で逆変換し、もう一方のフラグで逆変換した結果をｑ５（ｋ）とする。次に（Ａ）と同様に（１５）〜（１８）式でｌａ（ｉ）、ｌｂ（ｉ）を算出し、近傍領域の始点・終点の位置を算出する。

（Ｄ）Ｊ５軸が正の値から特異点に近づいてきて近傍領域を跳び越した場合
まず、第ｋ回目の粗補間の地点でＦ／ＮＦに関するもう一方のフラグ（Ｆｌｉｐ）で逆変換し、もう一方のフラグで逆変換した結果をｑ５（ｋ）とする。次に（Ｂ）と同様に（１９）〜（２２）式でｌａ（ｉ）、ｌｂ（ｉ）を算出し、近傍領域の始点・終点の位置を算出する。

（Ａ）、（Ｂ）いずれの場合も、第ｋ回目の周期で初めて近傍領域に入ったとき、第ｋ回目の周期での逆変換結果は元の逆変換結果もしくは切替えたフラグでの逆変換結果ではなく、近傍領域始点からの直線上の距離に応じた逆変換結果（例えば第ｋ回目の周期の移動量演算手段の出力が近傍領域の始点〜終点の距離のａ／ｂの位置ならば、近傍領域の始点の逆変換結果と近傍領域の終点の逆変換結果を関節補間で結ぶ場合のａ／ｂの地点の値）とする。以後の周期でも近傍領域の終点を通過するまでは近傍領域始点からの直線上の距離に応じた逆変換結果を採用する。

また（Ｃ）、（Ｄ）のいずれの場合も、第ｋ回目の周期で近傍領域を跳び越した場合、切替えたフラグでの逆変換結果を第ｋ回目の周期の逆変換手段７の出力として採用する。

逆変換手段４も、逆変換手段７と同様に、加減速手段の出力が特異点近傍領域の始点に到達するまでは動作開始地点で指定されているフラグを用いて逆変換する。加減速手段３の出力が特異点近傍領域にある場合は、特異点近傍領域の始点及び終点の各軸位置に基づいて逆変換を行う。具体的には、近傍領域始点からの直線上の距離に応じた逆変換結果（例えば加減速手段３の出力が近傍領域の始点〜終点の距離のａ／ｂの位置ならば、近傍領域の始点の逆変換結果と近傍領域の終点の逆変換結果を関節補間で結ぶ場合のａ／ｂの地点の値）とする。さらに、加減速手段３の出力が特異点近傍領域の終点を通過後は、特異点に応じて切り替えたフラグで逆変換を行う。

したがって、実施の形態１によれば、メカニカルシステム先端軌跡上の特異点近傍領域の始点と終点の間の区間では、当該始点及び終点の関節変位に基づいて各関節の位置指令を算出するようにしたので、特異点近傍を通過する際、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制できるため、動作時間を短縮できる効果がある。

実施の形態２．
実施の形態１とは、特異点近傍領域内にある場合の、逆変換手段４及び逆変換手段７での逆変換方法のみ異なっている。そのため、特異点近傍領域内にある場合の、逆変換手段４及び逆変換手段７での逆変換方法についてのみ説明する。

逆変換手段４では、加減速手段３の出力の先端軌道上の位置が近傍領域の始点〜終点の距離のａ／ｂの位置ならば、メカニカルシステムの先端３軸の関節位置指令は、近傍領域の始点の先端３軸の関節位置と近傍領域の終点の先端３軸の関節位置を関節補間で結ぶ場合のａ／ｂの地点の値とする。残りの軸の関節位置指令は、先端３軸の関節位置指令を用いて先端位置が始点〜終点の距離のａ／ｂの位置となるように定める。

逆変換手段７では、移動量演算手段６の出力の先端軌道上の位置が近傍領域の始点〜終点の距離のａ／ｂの位置ならば、メカニカルシステムの先端３軸の関節位置指令は、近傍領域の始点の先端３軸の関節位置と近傍領域の終点の先端３軸の関節位置を関節補間で結ぶ場合のａ／ｂの地点の値とする。残りの軸の関節位置指令は、先端３軸の関節位置指令を用いて先端位置が始点〜終点の距離のａ／ｂの位置となるように定める。

したがって、実施の形態２によれば、特異点近傍を通過する際、先端位置が本来の目標軌道からずれることを防止しながら、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制できるため、動作時間を短縮できる効果がある。

実施の形態３．
実施の形態３は、実施の形態１とは、近傍領域算出手段９及び近傍領域記憶手段１０と、近傍領域における逆変換手段４，７の動作が異なるのみであるので、近傍領域算出手段９及び近傍領域記憶手段１０及び逆変換手段４，７についてのみ説明する。

まず、実施の形態３では、２つ以上の特異点近傍領域が重なる場合を想定している。２つ以上の特異点近傍領域が重なる場合、後から入った方（第２）の特異点近傍領域の始点が先に入った方（第１）の特異点近傍領域にある時は、第２の特異点近傍領域始点算出の際、逆変換として第１の近傍領域内での逆変換を採用する。また、第２の特異点近傍領域の終点が第１の近傍領域内にある場合は、第２の特異点近傍領域終点算出の際、逆変換として第１の近傍領域内での逆変換を採用する。第２の特異点近傍領域の始点が第１の特異点近傍領域の始点よりも動作開始地点側にある場合は、第１の特異点に関するフラグが切り替わる前のフラグで第２の特異点近傍領域始点算出の逆変換を行う。第２の特異点近傍領域の終点が第１の特異点近傍領域の終点よりも動作終了点側にある場合、第１の特異点通過で切り替わったフラグ、もしくはさらに第２の特異点通過により切り替わったフラグで第２の特異点近傍領域終点算出の逆変換を行う。

第１の特異点近傍領域の始点・終点をそれぞれα１、β１、第２の特異点領域の始点・終点をそれぞれα２、β２とすると、同一の周期で近傍領域に入ったときは、α２のほうがα１よりも動作開始地点よりも近い場合があり得るので、動作開始地点から第１の特異点近傍領域の始点・終点、第２の特異点エリアの始点・終点を並べた場合、下記の４通りが考えられる。

（Ａ）α１、α２、β１、β２
（Ｂ）α１、α２、β２、β１
（Ｃ）α２、α１、β１、β２
（Ｄ）α２、α１、β２、β１

上記いずれの場合も片方の特異点近傍領域のみ、両方の特異点近傍領域、片方の特異点近傍領域のみ、の３区間が順番に存在するので、それぞれの区間内で関節補間もしくは３軸直交補間を行う。ここで、関節補間とは、それぞれの区間の始点〜終点の距離のａ／ｂの位置に先端位置指令があれば（逆変換手段４であれば加減速手段３の出力がそれぞれの区間の始点〜終点の距離のａ／ｂの位置にあり、逆変換手段７であれば移動量演算手段６の出力がそれぞれの区間の始点〜終点の距離のａ／ｂの位置にある）ならば、各区間の始点の逆変換結果と近傍領域の終点の逆変換結果を関節補間で結ぶ場合のａ／ｂの地点の値を各軸の関節位置指令とする方法である。また、３軸直交補間とは、それぞれの区間の始点〜終点の距離のａ／ｂの位置に先端位置指令があれば、先端３軸の関節位置指令は、各区間の始点の先端３軸の関節位置と各区間の終点の先端３軸の関節位置を関節補間で結ぶ場合のａ／ｂの地点の値とし、残りの軸の関節位置指令は先端３軸の関節位置指令を用いて先端位置が始点〜終点の距離のａ／ｂの位置となるように定める方法である。

第１の特異点がＦ／ＮＦの場合は、第１の特異点近傍領域にのみ入っており第２の特異点近傍領域に入っていない区間は３軸直交補間を行う。両方の特異点近傍領域に入っている区間、第２の特異点がＦ／ＮＦの場合は、Ｆ／ＮＦ近傍領域でも関節補間を行う。また、Ｆ／ＮＦ以外の特異点近傍領域区間でも関節補間を行う。

（Ａ）の場合は第２の特異点近傍領域の始点・終点α２、β２の算出を行う際、第１の特異点近傍領域の終点β１も再度計算する。（β２−β１）／（β２−α２）だけα２→β２の関節補間でβ２から戻った地点を新しいβ１とする。

（Ｂ）の場合は、β１のフラグを第２の特異点近傍領域でα２→β２で切り替わるフラグに応じてフラグを切替えて逆変換した結果を新しいβ１として採用する。

（Ｃ）の場合は、α２から（α１−α２）／（β２−α２）だけα２→β２の関節補間で進んだ地点を新しいα１とする。β２から（β２−β１）／（β２−α２）だけα２→β２の関節補間で戻った地点を新しいβ１とする。

（Ｄ）の場合は、α２から（α１−α２）／（β２−α２）だけα２→β２の関節補間で進んだ地点を新しいα１とする。また、β１のフラグを第２の特異点近傍領域でα２→β２で切り替わるフラグに応じてフラグを切替えて逆変換した結果を新しいβ１として採用する。

同一の補間周期で初めて複数の特異点近傍領域に入った場合、Ｆ／ＮＦ、Ｒ／Ｌ、Ａ／Ｂの順に優先度が高いとし、優先度の高い特異点領域に先に入ったとしてまず、優先度の高い特異点領域の始点・終点を計算した後、同一の補間周期でもう一方の特異点領域の始点・終点を計算する。第１の特異点近傍領域、第２の特異点近傍領域の計算方法は異なる粗補間周期で特異点近傍領域に入った場合と同一である。

したがって、実施の形態３によれば、特異点近傍を通過する際、複数の特異点近傍に同時に入る場合でも、複数の関節が大きく動くのを防止でき、先端速度の低下を抑制できるため、動作時間を短縮できる効果がある。

この発明の実施の形態１による指令値生成装置の概要を示すブロック図である。 図１の移動速度・移動量演算手段１の動作を説明するためのフローチャートである。 図２に連なるフローチャートである。 メカニカルシステムが垂直６軸ロボットの場合を示す説明図である。 垂直６軸ロボットのフラグ例１（Ｒｉｇｈｔ／Ｌｅｆｔ）の説明図である。 垂直６軸ロボットのフラグ例２（Ａｂｏｖｅ／Ｂｅｌｏｗ）の説明図である。 垂直６軸ロボットのフラグ例３（Ｆｌｉｐ／ＮｏｎＦｌｉｐ）の説明図である。

符号の説明

１ 移動速度・移動量演算手段、２ 移動速度修正手段、３ 加減速手段、４ 逆変換手段、５ 移動速度演算手段、６ 移動量演算手段、７ 逆変換手段、８ 特異点近傍領域突入判別手段、９ 近傍領域算出手段、１０ 近傍領域記憶手段。

Claims (3)

1. 複数の関節を備えたメカニカルシステムの先端を、各関節の駆動軸を制御することにより、予め定められた軌跡に沿って移動させる際の各関節の駆動軸を制御するための指令値を生成する指令値生成装置であって、
   加減速前のメカニカルシステム先端位置指令を演算する移動速度・移動量演算手段と、 前記移動速度・移動量演算手段で演算するメカニカルシステム先端位置指令を各関節の位置指令に変換する第１の逆変換手段と、
   加減速後のメカニカルシステム先端位置指令を各関節の位置指令に変換する第２の逆変換手段と、
   前記第１の逆変換手段からの関節変位に基づいて予め指定される特異点近傍領域にメカニカルシステムの先端位置指令が入るもしくは通過したことを判別する特異点近傍領域突入判別手段と、
   前記特異点近傍領域突入判別手段で特異点近傍領域に入るもしくは通過したと判別した場合に、当該動作におけるメカニカルシステム先端軌跡上での特異点近傍領域の始点と終点を算出する近傍領域算出手段と、
   前記近傍領域算出手段により算出した始点及び終点を記憶する近傍領域記憶手段と
   を備え、
   前記第１と第２の逆変換手段は、前記近傍領域記憶手段に記憶されるデータに基づいてメカニカルシステム先端軌跡上の特異点近傍領域の始点と終点の間の区間では、当該始点及び終点の関節変位に基づいて各関節の位置指令を算出する
   ことを特徴とする指令値生成装置。
2. 請求項１に記載の指令値生成装置において、
   前記第１と第２の逆変換手段は、メカニカルシステム先端軌跡上の特異点近傍領域の始点と終点の間の区間では、先端３軸の関節位置指令を、始点及び終点の関節変位を補間して算出し、残りの軸の位置指令を、先端３軸の関節位置指令を用いて先端位置が指令された軌跡上にあるように定める
   ことを特徴とする指令値生成装置。
3. 請求項１に記載の指令値生成装置において、
   前記第１と第２の逆変換手段は、２つ以上の特異点に対応する特異点近傍領域が重なる場合に、特異点近傍領域を３つ以上の区間に分割し、それぞれの区間の先端軌跡上での始点、終点を算出する
   ことを特徴とする指令値生成装置。

Images