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

Description

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

以下、本願では、複数の関節を備えたメカニカルシステムとしてロボットを例に説明するものとする。また、上記関節または関節の駆動軸という用語は、本願明細書では、便宜上、時として、その同義語として単に軸と表現する場合もある。
ところで、例えば、特許文献１にあるように、ロボットの制御では、特異点の存在を考慮しなければならない。即ち、先端を予め定められた軌道に沿って動作させる際に、特異点の近傍では少なくとも１つの軸の指令が急激に変化し、特異点に一致した地点では、先端位置から各軸変位を求める逆変換の解が無数に存在することになる。
そして、同文献では、特異点近傍を含む直線補間を行う際、関節角指令の差分が規定値以上になった場合に特異点と判断し、逆変換の解の種類を切り替えて、急激な関節角指令の変化を防止する。また、関節の最高速度を超過する場合は、サンプリング周期を長くして関節の最高速度以内で動作可能としている。

特開昭５８−１１４８８８号公報

特異点そのものを通過する場合は逆変換の解を切り替えれば急激な関節角指令変化を防止できるが、特異点近傍を通過する場合は解を切り替えても急激な関節角指令変化を防止できない場合がある。
また、関節の最高速度を超過することをサンプリング周期を長くすることにより防止しているが、速度超過を防止するために急激にサンプリング周期を変化させると許容加速度を超過し、実現できない、あるいは実現できても大きく振動するなどの問題があった。

以上のように、ロボットの制御には、特異点の存在から指令速度の急変が避けられない。また、各軸の駆動機構は、通例の駆動機構と同様、速度指令の急変には円滑に応動しにくい。従って、駆動機構の駆動特性等を考慮して駆動機構に入力する速度指令の急変を緩和する加減速手段を挿入するのが一般的に望ましい。しかし、加減速手段というものは、上述の、ロボット特有の条件を具備する補償はない。即ち、一般に、加減速手段は平均化処理の機能を備えたフィルタ等で構成するが、入力された速度制限の時間変化を緩和するが、入力された速度指令の最低値はむしろ増大して出力される可能性がある。
従って、従来のロボット制御では、過渡的に各軸で速度制限を越えて操作に支障を来したり、演算された速度指令よりもかなり低目の速度で操作して、動作全体を遅くする必要がある問題があった。

この発明は、以上のような問題点を解消するためになされたもので、各軸の速度制限を超過する可能性を抑制するとともに、過度な低速操作を無用として、高速円滑な操作を実現する指令値生成装置を得ることを目的とする。

この発明に係る第１の指令値生成装置は、複数の関節を備えたメカニカルシステムの先端を、上記各関節の駆動軸を制御することにより、予め定められた軌跡に沿って移動させるものにおける、上記各関節の駆動軸を制御するための指令値を生成する指令値生成装置であって、
上記各関節の駆動軸の速度制限を満足するように、上記先端の移動速度およびこの移動速度を積分して得られる上記軌跡に沿った上記先端の移動量を演算する移動速度・移動量演算手段、この移動速度・移動量演算手段における任意の演算時点ｔで演算された移動速度（ｔ）および移動量（ｔ）に対し、上記演算時点ｔを含む所定演算時間範囲内または上記移動量（ｔ）を含む所定演算距離範囲内での移動速度の演算結果に上記移動速度（ｔ）より小さい移動速度が存在しない場合は上記移動速度（ｔ）を踏襲し、上記移動速度（ｔ）より小さい移動速度が存在する場合は上記移動速度（ｔ）を当該小さい移動速度に置換した修正移動速度を作成し、この修正移動速度を積分して得られる移動量が上記移動量（ｔ）に達するまで出力する移動速度修正手段、上記各関節の駆動軸の加減速駆動特性を円滑化するため上記移動速度修正手段からの修正移動速度に波形処理として移動平均フィルタ処理を施し先端移動速度指令として出力する加減速手段、およびこの加減速手段からの上記先端移動速度指令を積分して得られる先端移動量指令を実現する上記各関節の変位指令を算出（以下、逆変換と称す）し上記変位指令から各関節の駆動軸を制御するための位置指令値または速度指令値を生成する第１の逆変換手段を備えたものである。

また、この発明の第２の指令値生成装置は、６関節のメカニカルシステムにおいて、先端移動量指令を生成する先端移動量指令生成手段、５関節／６関節切替指令を発生する５関節／６関節切替手段、および６関節逆変換手段と第４関節の関節変位を所定の値に固定することにより５関節のメカニカルシステムとして動作させる場合の５関節逆変換手段とを設け、上記５関節／６関節切替手段からの５関節／６関節切替指令に基づき上記６関節逆変換手段または５関節逆変換手段のいずれかを使用して上記先端移動量指令から逆変換を行い各関節の変位指令を出力する逆変換手段を備えたものである。

以上のように、第１の指令値生成装置では、加減速手段の前段に移動速度修正手段を修正し、演算時点ｔを含む所定演算時間範囲内または移動量（ｔ）を含む所定演算距離範囲内での移動速度の演算結果に、移動速度（ｔ）より小さい移動速度が存在する場合は、移動速度（ｔ）を当該小さい移動速度に置換した修正移動速度を作成し、この修正移動速度を積分して得られる移動量が移動量（ｔ）に達するまで当該修正移動速度を送出するようにしたので、各軸の速度制限を超過する可能性がその分抑制され、結果として当初の要求指令に近い高速度で円滑な操作が可能となる。

また、第２の指令値生成装置では、６関節であるにもかかわらず５関節メカニカルシステムとして動作させることにより、６関節のもので問題となる第５関節の変位が０である特異点もしくはその近傍を通過する動作においても、先端速度を落とすことなく動作させることが可能となる。

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

移動速度・移動量演算手段１では、ロボット、工作機械などのメカニカルシステムの先端を指定された軌道に沿って動かす場合の軌道の各地点毎（移動量毎）にメカニカルシステムの各軸（各関節）の速度制限を満たし、かつ規定の速度以下となる移動速度を算出する。
移動速度修正手段２では、移動速度・移動量演算手段１で算出した移動速度に対して加減速を考慮した修正を加え、制御周期毎の加減速前の移動速度を算出する。
加減速手段３では、移動速度修正手段２で修正した制御周期毎の移動速度に対して２段の移動平均フィルタ処理を行い、制御周期ごとのメカニカルシステムの先端位置指令を生成する。
第１の逆変換手段としての逆変換手段Ａ４では、後述するフラグ決定手段１１から送信されるフラグ、即ち、逆変換を行う場合の変換関数を決めるフラグに基づいて、加減速手段３から出力されるメカニカルシステムの先端位置指令を各軸の位置指令に変換する逆変換を実施する。
補正手段１２では、逆変換手段Ａ４の出力をフラグ決定手段１１の出力に基づき必要に応じて修正を行い、各軸の位置指令を生成する。

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

以下、図２、３のフローチャートを適宜参照して移動速度・移動量演算手段１の動作を説明する。
kを、演算ステップの累積回数（k=1，2，3・・・）としたとき、2*k≦ft/stを満たす最大のkをk1とする。この実施の形態１では、動作開始（k=1)から2*k≦ft/st、即ち、k≦k1の間は（ステップＳ１でＹＥＳ）、２周期分、即ち、２回分の計算を1度に行う。そして、動作開始時には移動速度の初期値を規定値の２倍である2*v_oとし、k回目の周期における移動速度の初期値を2*v_k-1とする（ステップＳ３）。ここでk=1のときはv_k-1は規定値v_oとする。

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

次に、st秒での移動量
dl_k=2*v_k-1*st （１）
を移動量算出手段６で算出し、さらにdl_kの累積値l_kを算出し、動作開始地点からl_kだけ進んだ地点p_kを移動量算出手段６で算出する。次に、第２の逆変換手段としての逆変換手段Ｂ７で、p_kを逆変換し、必要に応じて再逆変換手段８での再逆変換、及び変換結果選択手段９で複数ある逆変換の解の選択を行うことによりp_kを逆変換した結果であるth_kを算出する。
なお、逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作については後ほど詳細を説明する。

th_kは、移動速度演算手段５にフィードバックされ、前回の演算における逆変換結果th_k-1から各軸毎の移動量
dth_k = th_k−th_k-1 （２）
の計算を移動速度演算手段５で実施する。さらに、dth_kの各軸成分とそれぞれの許容最大値dthmaxの2倍との比
rdth_k(i)＝|dth_k(i)|/(2*dthmax(i)) （３）
を算出（iはメカニカルシステムの第ｉ軸を意味する）し、rdth_k(i)の最大値rdth_kmaxを求める（ステップＳ３）。

rdth_kmax≦1の場合（メカニカルシステムの軸数をｎとし、1≦i≦nを満たすすべての自然数iに対して|dth_k(i)|≦2*dthmax(i)が成り立つ場合、ステップＳ４でＹＥＳ）は移動速度、移動量として1回目の計算結果をそのまま採用し、即ち、後述の速度操作をすることなく、また、上記で、２回分を１回で行った計算結果を本来のステップでの値に換算する処理を行う。
即ち、2*k-1番目の移動速度vs(2*k-1)および2*k番目の移動速度vs(2*k)をいずれもv_k-1とする（ステップＳ５）。
また、2*k-1番目の移動量ls(2*k-1)を、
ls(2*k-1)＝l_k-1+0．5*dl_k （４）
2*k番目の移動量を、
ls(2*k)= l_k （５）
とする。

また、v_k-1=v_oの場合は
v_k=v_o （６）
とする。また、v_k-1<v_oの場合は
v_k=min(v_k-1/rdth_kmax， v_o ) （７）
とする（ステップＳ６）。ここでmin()は最小値を意味している。
（７）式から分かるように、各軸の速度に余裕があってもv_o以上には速度を上げない。上限をv_oと設定しておくことにより、v_oで各軸速度が許容値を超えない場合は再度計算する必要がなく、計算量が削減する。仮に、少なくとも1つの軸の速度が許容値に達するように動作させる場合は、毎周期再度の計算が必要となり計算量が増大する。

rdth_kmax＞1の場合（ステップＳ４でＮＯ）は、いずれかの軸の速度が許容最大値を超えているので、移動速度を2* v_k-1/rdth_kmaxに修正し、dl_k及びl_kを算出し直す。そして、2*k-1番目の移動速度vs(2*k-1)および2*k番目の移動速度vs(2*k)をいずれもv_k-1/rdth_kmaxとする（ステップＳ７）。また、移動量に関しては求め直したdl_k及びl_kを用いて、2*k-1番目の移動量ls(2*k-1)を、
ls(2*k-1)＝l_k-1+0．5*dl_k （８）
また、2*k番目の移動量を、
ls(2*k)= l_k （９）
とする。また、
v_k =v_k-1/rdth_kmax （１０）
とする（ステップＳ８）。

2*k＞ft/stとなって以降、即ち、ｋ＞ｋ１になると（ステップＳ１でＮＯで、以下、図３のフローに移る）、今まで、２回分の計算を１度に実施していたのを、１度の計算で１回分を計算する処理に切り替える。
k>k1では第k番目の周期の演算をk2=k+kl番目の周期の演算とみなすことにし（ステップＳ９）、v_2*kl=v_kl、l_2*kl=l_klとする（ステップＳ１０）。

即ち、第kl周期（ここまでは2回分計算）では、2*kl-1番目のデータと2*kl番目のデータを計算している。第kl+1回目の周期（ここからは1回分計算）では1回分の計算をすることになるが、本当はkl+1回目の周期に相当するので、2*kl回目の周期まで1度に1回分の計算を行ってきたとみなしている。つまり、2*kl+1番目の周期であるとして計算する。
同様に、第kl+2番目の周期では、2*kl+2番目の周期であるとして計算する。これを一般的な表現で書けば、k+kl番目の周期とみなすことになる。また、本当はkl+1回目の周期であるのを2*kl+1番目の周期であるとして計算する場合、前回の速度はv_2*kl、前回の移動量はl_2*klとなるが、それらの値はそれぞれvs(2*kl)、ls(2*k)であるため、v_2*kl=v_kl、l_2*kl=l_klとなる。

次に、st秒での移動量dl_k2=v_k2-1*stを移動量算出手段６で算出し、さらにdl_k2の累積値l_k2を算出し動作開始地点からl_k2だけ進んだ地点p_k2を移動量算出手段６で算出する。
次に、逆変換手段Ｂ７でp_k2を逆変換し、必要に応じて再逆変換手段８での再逆変換、及び変換結果選択手段９で複数ある逆変換の解の選択を行うことによりp_k2を逆変換した結果であるth_k2を算出する。
なお、逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作については後ほど詳細を説明する。

th_k2は、移動速度演算手段５にフィードバックされ、前回の演算における逆変換結果th_k2-1から各軸毎の移動量dth_k2 = th_k2−th_k2-1の計算を移動速度演算手段５で実施する。さらに、dth_k2の各軸成分とそれぞれの許容最大値dthmaxとの比
rdth_k2(i)＝|dth_k2(i)|/(dthmax(i)) （１１）
を算出（iはメカニカルシステムの第ｉ軸を意味する）し（ステップＳ１１）、rdth_k2(i)の最大値rdth_k2maxを求める。

rdth_k2max≦1の場合は（ステップＳ１２でＹＥＳ）、移動速度、移動量として1回目の計算結果をそのまま採用し、k2番目の移動速度vs(k2)をv_k2-1とする（ステップＳ１３）。また、k2番目の移動量ls(k2)をls(k2)＝l_k2とする。
また、v_k2-1=v₀の場合は
v_k2=v₀ （１２）
とする。v_k2-1<v₀の場合は
v_k2 =min(v_k2-1/rdth_k2max， v₀ ) （１３）
とする（ステップＳ１４）。ここでmin()は最小値を意味している。

rdth_k2max＞1の場合（ステップＳ１２でＮＯ）は、移動速度をv_k2-1/rdth_k2maxに修正し、dl_k2及びl_k2を算出し直す。k2番目の移動速度vs(k2)をv_k2-1/rdth_k2maxとする（ステップＳ１５）。また、移動量に関しては求め直したdl_k2及びl_k2を用いて、ls(k2)＝l_k2とする。また、v_k2 =v_k2-1/rdth_k2maxとする（ステップＳ１６）。

次に、逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作について説明する。
逆変換手段Ｂ７で先端位置から各軸の位置を求める逆変換を実施する。一般に逆変換の解は複数存在し、どの解を算出するかを指定する必要がある。この解の指定のための変数をフラグとここでは呼ぶことにする。逆変換手段Ｂ７では、動作開始後、フラグの変更が行われるまでは動作開始地点で指定されているフラグを用いて逆変換を算出する。後述する再逆変換手段８、変換結果選択手段９の動作によりフラグが変更された場合は、以降の周期では変更されたフラグを用いて逆変換を算出する。

ここで、逆変換の解が1つに定まらない特異点と一致する場合の扱いについて説明する。なお、以下に、その処理方式、即ち、特異点と一致し通常では定まった解が得られないという問題を解消するための逆変換結果生成手段（図示せず）について提案するが、この方式は、逆変換手段Ｂ７に限らず、後段で更に詳しく取り上げる逆変換手段Ａ４についても適用出来るものである。

この特異点と一致する場合、２つの軸の値は任意の値をとりうる。特異点の一例としては、図４に示す６軸多関節ロボットの第５軸の角度が０になり、第４軸と第６軸が同一直線上に重なる場合があげられる。特異点と一致する場合は、逆変換手段Ｂ７では、図５のフローで各軸の位置を算出する。まず、特異点にあるために、任意の値を取りうる２つの軸の値を、前回の周期の速度をそのまま保つように一旦定める。例えば２つの軸がそれぞれ第ｉ軸と第ｊ軸でｋ回目の周期で特異点と一致したとすると、
thi(k) = thi(k-1)+(thi(k-1)-thi(k-2)) （１４）
thj(k) = thj(k-1)+(thj(k-1)-thj(k-2)) （１５）
で第ｉ軸及び第ｊ軸の位置を一旦定める（ステップＴ１）。即ち、この間では、両軸は、等速で動いていると仮定するわけである。

次に、今求めた第ｉ軸及び第ｊ軸の位置とメカニカルシステムの先端位置から残りの軸の位置を求める（ステップＴ２）。次に、求めた各軸の位置から先端の位置姿勢を算出し、先端位置姿勢誤差epを算出(ステップＴ３)する。さらに、thj(k)、thi(k)にep/2を加算することにより補正を行う（ステップＴ４）。即ち、上記で、両軸が等速で動いていると仮定したことによる誤差を補正するものである。

以上のように、この逆変換結果生成手段を備えることにより、解が一意に定まらない特異点に一致する場合も、逆変換が可能となり、既述したとおり、各種の逆変換手段に提要出来、同等の効果を奏するものである。

次に、移動速度・移動量演算手段１内の再逆変換手段８では、まず、逆変換手段Ｂ７で逆変換を行った結果が特異点近傍にあるかどうかの判別を行う。
特異点近傍であるかどうかの判別方法としては、ある特定の関節の変位が決まった範囲にあるかどうかで判別する方法が挙げられる。例えば、図４の６軸ロボットの第４軸、第６軸が同一直線上に重なる特異点の近傍かどうかは、第５軸の変位が０近傍かどうかで判別できる。例えば、しきい値を±５[deg]に設定した場合なら、第５軸の変位が±５[deg]の範囲にあれば特異点近傍であり、その範囲になければ特異点近傍ではないことになる。

特異点近傍にある場合は、逆変換手段Ｂ７で用いたフラグと近傍にある特異点に関して対になるフラグで逆変換を再度実施する。
再び、図４の６軸多関節ロボットの場合を例に説明すると、フラグは3bitで表現され、フラグの3bitの値の組み合わせ毎に逆変換の解が存在する。また、それぞれの特異点は、フラグの各bitに対応する。例えば、図４の６軸ロボットの第４軸、第6軸が同一直線上に重なる特異点は3bit目に対応（1bit目あるいは2bit目に対応させていても構わない）しているとすると、ある特異点近傍にあると判別された場合は、その特異点に対応するbitの値を変更して再逆変換手段８により再度逆変換を実施する。
例えば、図４の６軸ロボットの第４軸、第6軸が同一直線上に重なる特異点近傍にあると判別され、現在のフラグの3bit目の値が１の場合は、3bit目の値を０に変えたフラグで再度逆変換を実施する。
特異点近傍にない場合は何もしない。

変換結果選択手段９では、再逆変換手段８で再度逆変換が実施されなかった場合、すなわち特異点近傍になかった場合は何もしない。再逆変換手段８で再度逆変換が実施された場合は逆変換の解の選択を実施する。
以下、この変換結果選択手段９の動作について、図６のフローチャートを参照して説明する。
前回の周期の逆変換結果をtho、今回の周期の逆変換手段Ｂ７における逆変換結果をtha、再逆変換手段８での逆変換結果をthbとする。また、０以上1以下の定数waを用いて
thc = wa*thb+(1-wa)*tha （１６）
を定義する。即ち、thcは、定数waを用いて、両結果tha、thbに重み付けをして加算したものである。
ここで、thcを導入するのは、先端の軌跡が本来の値から多少ずれても高速に動作させたいためである。

ステップＵ１で現在のフラグをそのまま用いた場合の各軸移動量
dtha=tha-tho （１７）
を算出する。ステップＵ２では、dthaの各軸要素の少なくとも１つが規定値以上であるかどうかの判別を行う。少なくとも１つが規定値以上の場合はステップＵ３、そうでない場合はステップＵ４に進む。

ステップＵ３では、移動量対応の選択を実施する。ステップＵ３では、tha、thb、thcを逆変換結果として選択した場合の各軸の移動量の絶対値の最大値maxdtha， maxdthb， maxdthcを算出する。ここで、maxdthaはdthaの各軸要素の絶対値の最大値である。ステップ３では、以下の条件１〜３の判別を順に行い、判別結果に応じたフラグ及び逆変換結果を選択する。

条件１：maxdthb<maxdthaの場合は、フラグを切り替えてthbを逆変換結果として採用する。条件１でフラグを切り替えてthbを採用するのは、そちらの方が先端速度を落とす必要がなく、高速に通過できるためである。
条件１でフラグを切り替えた場合は、次回の周期以降では逆変換手段Ｂ７では切り替えたフラグ（thbのフラグ）で逆変換を実施する。
但し、thbが、動作範囲外、即ち、少なくとも1つの関節軸の変位がその上限もしくは下限を超過することになる場合はフラグを切り替えず、逆変換結果もthaを選択する。

条件２：条件１が成立せず、maxdthc<maxdthaの場合は、フラグは切り替えないが、（１６）式で求めたthcを逆変換結果として採用する。
条件２でフラグを切り替えずにthcを採用するのは、tha、thbいずれを採用するよりも先端速度を落とす必要がなく、高速に通過できるためである。
但し、thcを採用すると先端が本来の軌跡からずれるため、できるだけtha、もしくはthb（この両者なら先端は本来の軌跡を保つ）が採用されるように、条件１による処理を優先して実施する。
また、thcを採用しても逆変換手段Ｂ７では元のフラグ（＝thaのフラグ）で逆変換を実施する。
以上のように、条件１，２は、先端の速度をできるだけ落とすことがないようにする視点でフラグを選択しているわけである。
但し、thcが動作範囲外となる場合は、逆変換結果もthaを選択する。

条件３：条件１、２がいずれも成立しない場合はステップＵ４へ進む。
先のステップＵ３では関節の速度が高くなり過ぎないようにフラグを切替えたが、ステップＵ４、Ｕ５では動作範囲外に出ることを防止するためにフラグを切り替える。
先ず、ステップＵ４では、現在のフラグのままでは近いうちに動作範囲外に出るかどうかの判別を行う。
動作範囲外に出るかどうかは、thaがリミット近傍にあるかどうかで判別する。具体的には、第ｉ軸の関節変位q(i)がリミット近傍にあるかどうかは、全軸共通のパラメータをα、第ｉ軸の＋側リミットをqmax、−側リミットをqminとして、q(i)>qmax-α もしくはq(i)<qmin＋αが成立する場合にリミット近傍にあると判定する。

すべての軸の変位がリミット近傍になければ、そのフラグで逆変換を続けても動作範囲外になることはないため、逆変換手段Ｂ７現在のフラグで逆変換したthaを選択する。少なくとも1つの軸の変位がリミット近傍にある場合は、近いうちに動作範囲外になる可能性があるため、ステップＵ５でフラグの切替を検討する。

ステップＵ５では、thbがリミット近傍にあるかどうかを判別する。即ち、再逆変換手段８により、切り替えたフラグで逆変換を実施した結果が動作範囲内にあるかどうかを判別する。
これは、ステップＵ４で動作範囲外と判別されるもとになった軸がフラグを切り替えることにより値の変わらない軸（例えば、図４の６軸ロボットの第４軸、第６軸が同一直線上に重なる特異点近傍の場合、第１，２、３軸の変位はフラグを切り替えても変わらない）の場合は値が変わらないためthbも動作範囲外になる。
また、ステップＵ４で動作範囲外と判別されるもとになった軸がフラグを切り替えることにより値の変わる軸であっても、その軸の上限値、下限値によってはフラグを切り替えても動作範囲外になる場合がある。そのためステップＵ５でフラグを切り替えても動作範囲外になるかどうかの判別をする。

thbがリミット近傍にない場合は、フラグを切り替えてthbを逆変換の解として選択する。フラグを切り替えることにより動作範囲外でロボットを停止させる必要がなくなるため、動作できる範囲が広がる。
ステップＵ５で、thbもリミット近傍にある場合、即ち、フラグを切り替えても動作範囲外になる場合は切り替えても仕方がないので、フラグは切り替えずthaを逆変換の解として選択する。
変換結果選択手段９においてフラグを切り替えた場合、及びthcを選択した場合に、その周期及び1つ前の周期の移動量と逆変換結果を切替地点記憶手段１０に記憶する。

次に、移動速度修正手段２について説明する。
移動速度・移動量演算手段１では、上記のような作用をしているので、各区間(ls(k-1)〜ls(k))における各軸の関節速度の制約を満たす先端速度vs(k)とvs(k)の累積値であるls(k)が出力される。
ロボット、工作機械などのメカニカルシステムでは機械剛性が十分には高くなく、また、メカニカルシステムを駆動するモータのトルクにも制限があるため、移動速度・移動量演算手段１の演算結果をそのまま先端での位置指令として用いることはできず、加減速を行う必要がある。
加減速を行うための手段として移動平均フィルタ（本願では、加減速手段３が相当する）を用いる方法が知られているが、移動平均フィルタを用いる場合、フィルタの出力の最小値はフィルタ入力の最小値よりも大きな値になる。これは移動平均フィルタでは、最小値の前後を含めて平均値が出力されるため、その出力は元々の最小値より大きな値になるためである。

そこで、各区間(ls(k-1)〜1s(k))での加減速手段３への入力を、移動平均フィルタの窓長を考慮した前後の区間での最小値に設定するようにしておけば、加減速手段３の出力が、その区間(ls(k-1)〜ls(k))にある場合は、超えてはいけない先端速度vs(k)以下となる。
区間(ls(k-1)〜ls(k))で、先端速度がVS(k)以下であれば、加減速後の速度が各区間で移動速度・移動量演算手段１での速度以下となるので、各軸の関節速度の制約を常に満たすことができる。
先端の移動量が各区間(ls(k-1)〜ls(k))にあるときの加減速手段３への入力を移動平均フィルタの窓長を考慮した前後の区間での最小値に設定するようにするために、移動速度修正手段２では下記の作用を行う。図７のフローチャートを参照して説明する。

移動速度修正手段１では、移動量演算手段６で移動量を算出する際に用いる時間刻みをst秒、移動平均フィルタの窓長の総和をft秒とするとき、ft/st/2以上の最小の整数をfilaとし、移動量演算手段６で算出した移動速度を修正し、指令値生成周期ごとの加減速前の位置指令もしくは速度指令を生成する。

先ず、移動速度・移動量演算手段１が出力する移動量ls(k)に対応する移動速度vlmin(k)を以下のように算出する（ステップＶ１）。
k>filaの場合は
vlmin(k) = min(vs(k-fila+1)， vs(k-fila+2)． ・ ・ ，vs(k+fila)) (１８)
で算出し、
k≦filaの場合は
vlmin(k)=min(vs(1)、 vs(2)、・・vs(2k)) (１９)
で算出する。
(１８)、(１９)式の演算は、各区間(ls(k-1)〜ls(k))における最小値の演算を行っている部分である。

次に、移動速度修正手段２での第１周期での出力である速度指令vt(1)をvlmin(1)*ovrdとする。ここで、ovrdは、メカニカルシステムを動作させるためのプログラムで設定される速度比率であり、ovrd=1のときは100%の速度で動作させることを意味する。またls(0)=0としている。
移動速度修正手段２の第ｊ周期での出力である速度指令vt(j)のj-1までの累積値lt(j-1)=(vt(l)+vt(2)+・・・+vt(j-1))*stが、ls(k-l)<lt(j-1)≦ls(k)の間にある場合は、
vt(j)=vlmin(k)*ovrd (２０)
とする。
lt(j-1)が、ls(k)を超えて次の区間に入っている場合、即ち、ls(k)<lt(j-1)≦ls(k+1)となる場合は、
vt(j)=vlmin(k+l)*ovrd (２０ｂ)
とする（ステップＶ２〜Ｖ５）。

これは、移動速度修正手段２の出力を速度指令とする場合に、移動速度修正手段２の出力、即ち、加減速手段３の入力の累積値が、区間(ls(k-l)〜ls(k))にある間はその最小値vlmin(k)とするものである。
換言すれば、vt(j)の前回までの累積値が存在する区間に対応する最小値をvt(j)に設定するわけである。
(２０)式で計算されるvt(j)を移動速度修正手段２から出力する速度指令とする。移動速度修正手段２から位置指令を出力する場合は、vt(j)の累積値lt(j)を位置指令とする。

加減速手段３では、移動速度修正手段２から出力される指令が速度指令の場合は、速度指令を２段の移動平均フィルタに通し、通した結果を累積して指令移動量を算出し、指令移動量と動作開始地点の位置から先端の位置指令を算出する。移動速度修正手段２から出力される指令が位置指令の場合は、位置指令を２段の移動平均フィルタに通して指令移動量を算出し、指令移動量と動作開始地点の位置から先端の位置指令を算出する。

フラグ決定手段１１では、切替地点記憶手段１０の記憶内容と加減速手段３で算出された指令移動量に基づいて逆変換手段Ａ４でのフラグを決定する。変換結果選択手段９でフラグが変更されていない場合は、フラグ決定手段１１からは動作開始地点で指定されているフラグが出力される。変換結果選択手段９でフラグが変更されている場合は、加減速手段３から出力される指令移動量が切替地点記憶手段１０に記憶されている切替地点に到達するまでは切替前のフラグを逆変換手段Ａ４に送信し、加減速手段３から出力される指令移動量が切替地点記憶手段１０に記憶されている切替地点を通過後は切替後のフラグを送信する。

補正手段１２では、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替直前の地点とフラグ切替地点の間にある場合にフラグ切替により値が変わる軸の指令を補正する。
ここで補正処理を行うのは、移動速度・移動量演算手段１でフラグを切り替えている区間にある場合、加減速手段３の後に移動速度・移動量演算手段１と同じフラグ切替手段（逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９）を設ける方法もあるが、同じ区間でも移動速度・移動量演算手段１での速度と加減速手段３の出力では速度が異なるため、フラグ切替の挙動が異なる場合があり、これらを考慮したものである。

フラグ決定手段から、フラグ切替直前の地点の移動量lfb、逆変換結果thfbとフラグ切替地点における移動量lf、逆変換結果thfを入力する。フラグが切り替わることにより値が変わる軸の指令を
thnow(i)=(thf(i)-thfb(i))*(lnow-lfb)/(lf-lfb)+thfb(i) （２１）
に置き換える。ここでlnowは加減速手段で算出された指令移動量である。

また、ｉは、第ｉ軸を示す。即ち、図４の６軸ロボットの第４軸、第６軸が同―直線上に重なる特異点の場合は、フラグが変わることにより値が変わる軸は、第４軸、第５軸、第６軸である。従って、（２１）式の計算は、第４軸、第５軸、第６軸の要素に対して実施する。
また、（２１）式は、フラグ切替直前地点とフラグ切替地点との間における移動量変化量の両出力（加減速手段３からの出力とフラグ決定手段１１からの出力）比は、同地点間における逆変換結果変化量の同両出力比に等しいと仮定して求められている。

本発明の実施の形態１によれば、メカニカルシステムの先端を予め定められた軌道に沿って動作させる軌道補間動作を行う際、関節速度の超過及び急激な加減速を引き起こすことなく、また、必要以上に先端速度を低下させることなく動作でき、メカニカルシステムの動作時間を短縮できる効果がある。
また、指令軌道を変更することにより、メカニカルシステムの動作時間を短縮できる効果がある。
さらに、特異点近傍で動作範囲外になる場合に再逆変換を実施し、もう一方の逆変換結果を採用することにより動作範囲外になることが防止でき、軌道補間で動作可能な範囲を拡大できる効果がある。
また、特異点と一致する場合にも各軸の指令を一意に定めることができる効果もある。

実施の形態２．
実施の形態１では、移動速度・移動量演算手段１での速度規定値としてはメカニカルシステムに固有の先端最高速度v₀を用い、メカニカルシステムを動作させるためのプログラムで設定される速度比率は、移動速度修正手段２の（２０）式で考慮した。
これに対し、実施の形態２では、移動速度・移動量演算手段１での速度規定値としてovrd* v₀を用い、移動速度修正手段２は（２０）式の代わりに
vt=vlim(k) （２２）
とする。

即ち、実施の形態１では、ovrdを移動速度修正手段２で考慮するので同じ軌道を通る場合ならば、ovrdの設定によらず移動速度・移動量演算手段１での動作は全く同じになる。従って、移動速度・移動量演算手段１でフラグが切り替わるかどうかと切り替わる場合の切り替わる地点もovrdによらず同じになる。

一方、実施の形態２では、ovrdを移動速度・移動量演算手段１で考慮する。そのため、例えば、ovrd=0．1の場合、移動速度・移動量演算手段１で算出する移動速度は、ovrd・1の場合の0．1倍になる。移動速度が0．1倍になっているため、ovrd=lでは各軸移動量の各軸要素の少なくとも１つが規定値以上となる場合でも、ovrd・0．1では各軸移動量がovrd＝1の場合の0．1倍になるため、いずれの軸の要素も規定値を超えないことがある。
そのため、ovrd=lではフラグの切替が起こるが、ovrd=0．lではフラグの切替が起こらない場合がある。ロボットを動作させる際、最初はovrdを小さくして試験動作を行い、試験動作でＯＫならovrdを大きくして実際の作業を行うことがあるが、ovrdの値によりフラグが切り替わるかどうかが異なると試験動作と実際の動作とがー致しなくなり試験動作での評価が難しくなる短所がある。
しかし、実際のovrdに合わせてフラグの切り替えの判断をするため、そのovrdに応じたフラグ切り替えの判断が実施できる。反面、実施の形態１では、ovrd＝1の速度で判断するため、ovrdが小さくフラグを切り替える必要のない場合でもフラグを切り替えることがあり、この点は実施の形態２が実施の形態１より有利と言える。

実施の形態３．
この実施の形態３では、実施の形態１で説明した移動速度修正手段２の変形例について説明する。実施の形態３は、実施の形態１とは、vlmin(k)の算出方法が異なっている。
先ず、vsの増減は常時チェックし、最新の極小値vs(kp)を記憶しておく。ここで、極小値とは、vs(m-l)>vs(m)<vs(m+1)が成立するときのvs(m)をいう。
k>filaの場合は、k-fila+1〜k+filaの区間に極小値kpがある場合は、
vlmin(k)=min(vs(k-fila+1)， vs(kp)， vs(k+fila)) （２３）
とし、極小値kpがない場合は
vlmin(k)=min(vs(k-fila+1)， vs(k+fila)) （２４）
とする。
k≦filaの場合は、1〜2kの区間に極小値kpがある場合は、
vlmin(k)=min(vs(1)， vs(kp)， vs(2k)) （２５）
とし、極小値kpがない場合は
vlmin(k)=min(vs(1)， vs(2k)) （２６）
とする。

上記で、「最新の」とは、例えば、k=5とk=15で極小値になったとした場合、k+filaもしくは2kが４以下の時は極小値なし、k+filaもしくは2kが5〜14のときはvs(5)を記憶し、k+filaもしくは2kが15以上のときはvs(15)を記憶する。
従って、vlmin(k)を求めるもととなるvs（）の極小値が２つ以上ある場合は、新しいほうの極小値のみ考慮する。最新の極小値のみ考慮する構成では、場合によっては最小値とはならない場合があるという短所があるが、min（）を求めるもととなる要素数が少ないため計算量は少なくてすむ長所がある。
一方、実施の形態１では、（１８)、(１９)式で必ず最小値を求める。従って、計算量が多い短所はあるが、常に最小値を求めることができる長所がある。

なお、移動速度修正手段の更なる変形例としては、以下のものが考えられる。即ち、
実施の形態３では、所定の演算時間範囲内での最新の極小値と同期間端での速度から最小のものを選ぶようにしたが、同期間端の値より小さい値が存在すればその値を採用するようにしても良い。この場合も、効果の度合いは異なるが、移動速度修正手段を挿入することによる、上述したと同様の効果を奏すると言える。
更に、最小値や最近の極小値等を検出する対象範囲として、実施の形態１や３と異なり、所定の演算距離の範囲としても、移動平均フィルタ等との設定を別途調整する必要があるが、同等の効果が得られることは明らかである。
従って、移動速度修正手段２としては、移動速度・移動量演算手段１における任意の演算時点ｔで演算された移動速度（ｔ）および移動量（ｔ）に対し、上記演算時点ｔを含む所定演算時間範囲内または上記移動量（ｔ）を含む所定演算距離範囲内での移動速度の演算結果に上記移動速度（ｔ）より小さい移動速度が存在しない場合は上記移動速度（ｔ）を踏襲し、上記移動速度（ｔ）より小さい移動速度が存在する場合は上記移動速度（ｔ）を当該小さい移動速度に置換した修正移動速度を作成し、この修正移動速度を積分して得られる移動量が上記移動量（ｔ）に達するまで出力するものであればよいと言える。

以上のように、この発明の実施の形態３における指令値生成装置では、上記所定演算時間範囲内または所定演算距離範囲内での移動速度の演算結果の内の最新極小値を採用するようにしたので、移動速度修正手段での演算が簡便となるという効果がある。

実施の形態４．
図８は、本発明の実施の形態４による指令値生成装置を示すブロック図である。実施の形態１とは、逆変換手段Ａ４及び補正手段１２の代わりに、第１の逆変換手段としての逆変換手段Ｃ１３を用いることが異なっているので、以下、この逆変換手段Ｃ１３を中心にその動作について説明する。
逆変換手段Ｃ１３では、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替直前の地点に到達するまでは切替地点以前のフラグで逆変換を実施する。また、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替を通過後は切替地点以後のフラグで逆変換を実施する。
加減速手段３で算出された指令移動量がフラグ切替直前の地点と切替地点の間にある場合は、先ず、前出の（２１）式と同内容の
thnow=(thf-thfb)*(lnow-lfb)/(lf-lfb)+thfb （２７）
の算出を行い、フラグ切替により値が変わる軸の値を定める。
次に、先端の姿勢は無視し、lnowに対応する先端位置と今求めたフラグ切替により値が変わる軸の値より、残りの軸の値を算出する。

即ち、更に具体的に説明すると、実旅の形態１では、逆変換実施後に一部の関節軸（例えば、図４の第５軸の関節変位が０である特異点の場合の第４，５，６軸）の値を変更する。そのため、変更後の各関節変位から求めた先端位置はもともとの指令とは一致しなくなる。即ち、先端位置が本来の軌跡からずれる短所がある。しかし、一部の関節軸の関節変位を修正するだけであるので、図４の６軸ロボットの場合、３通りある特異点すべてで同じように、かつ少ない計算量で計算できる利点がある。

一方、この実施の形態４では、修正する軸の関節変位を求めてから残りの軸の関節変位を算出するので、先端位置は本来の軌跡を保つという利点がある。先端姿勢は本来の姿勢は保てないが、この点は、実施の形態１も同様である。
図４のような６軸ロボットの逆変換を行う場合、第４，５，６軸の変位を求めてその次に残りの軸（＝第１，２，３軸）の変位を算出することが一般的である。従って、第５軸の変位が０の特異点に関しては第４，５，６軸の変位が先に定まっても通常の逆変換と同じになり残りの軸の変位は少ない計算量で算出できる。しかし、残りの特異点に関しては、第４，５，６軸のうち少なくともｌつの値は定まっていないため残りの軸の値を通常の逆変換と同じ方法で算出することはできず、繰り返し計算を行うなどの必要が生じ、計算量が増加する短所がある。

以上のように、この発明の実施の形態４における指令値生成装置では、フラグ切替直前地点とフラグ切替地点との両地点間では、上記先端位置が定められた軌跡を維持するように、当該切替で逆変換結果である関節変位が変化する関節の関節変位に基づき他の関節の関節変位を求めるようにしたので、フラグを切り替える区間においても、先端位置は本来の軌跡を保つという効果がある。

実施の形態５．
この実施の形態５は、先の実施の形態４の逆変換手段Ｃ１３の変形例で、回路ブロック図としては、図８がそのまま適用される。以下、逆変換手段Ｃ１３を中心にその動作について説明する。
逆変換手段Ｃ１３では、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替直前の地点に到達するまでは切替地点以前のフラグで逆変換を実施する。また、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替を通過後は切替地点以後のフラグで逆変換を実施する。
加減速手段３で算出された指令移動量がフラグ切替直前の地点と切替地点の間にある場合は、先ず、前出の（２７）式の算出を行い、各軸の値を一旦算出する。
次に、当該一旦算出した関節変位に対応するフラグを求め、先端位置から当該フラグを使用して逆変換を行い各関節変位を算出する。

即ち、更に具体的に説明すると、（２７）式で算出した各軸の変位に対応するフラグを求める。例えば、図４の６軸ロボットにおいて、フラグ切替前は、第５軸が＋の値を持つフラグ（ここではフラグ１と呼ぶことにする）で、フラグ切替後は、第５軸が−の値を持つフラグ（ここではフラグ２と呼ぶことにする）になる場合、（２７）式において、第５軸の値が＋の場合はフラグを１とし、第５軸の値が−の場合はフラグを２とする。
次に、現在の先端位置姿勢から（２７）式の結果に応じて選択されたフラグを用いて逆変換を行い、各軸の変位を算出する。実施の形態５の逆変換手段Ｃ１３において、加減速手段３で算出された指令移動量がフラグ切替直前の地点と切替地点の間にある場合のフローを図９に示す。

実施の形態５と実施の形態１とを比べると、実施の形態５では、フラグを切り替える区間においても先端位置姿勢を本来の位置姿勢に保つことが可能になる長所がある。
一方、フラグを切り替える際に、実施の形態１と比べると速度の変動が大きくなり振動を誘発する可能性があるという短所がある。

以上のように、この発明の実施の形態５における指令値生成装置では、フラグ切替直前地点とフラグ切替地点との両地点間では、上記フラグ決定手段から入力した上記両地点での逆変換結果である各関節変位と先端位置に基づき各関節変位を一旦算出し、当該一旦算出した関節変位からその関節変位に対応するフラグを求め、上記先端位置から当該フラグを使用して逆変換を行い各関節変位を求めるようにしたので、フラグを切り替える区間においても先端位置姿勢を本来の位置姿勢に保つことが可能になるという効果がある。

実施の形態６．
この実施の形態６は、先の実施の形態４の逆変換手段Ｃ１３の変形例で、回路ブロック図としては、図８がそのまま適用される。また実施の形態６では、制御対象を先の図４の６軸垂直多関節ロボットとする。以下、逆変換手段Ｃ１３を中心にその動作について説明する。

逆変換手段Ｃ１３では、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されいるフラグ切替直前の地点に到達するまでは切替地点以前のフラグで逆変換を実施する。また、加減速手段３で算出された指令移動量が切替地点記憶手段１０に記憶されているフラグ切替を通過後は切替地点以後のフラグで逆変換を実施する。

加減速手段３で算出された指令移動量がフラグ切替直前の地点と切替地点の間にある場合は、フラグ決定手段から入力した上記両地点での逆変換結果である各関節変位と先端位置に基づき各関節変位を一旦算出し、第１、２、３関節以外の関節変位は当該一旦算出した値に決定し、当該決定した第１、２、３関節以外の関節変位と上記先端位置から第３関節の先端位置を算出し、当該一旦算出した関節変位からその関節変位に対応するフラグを求め、上記第３関節の先端位置から当該フラグを使用して逆変換を行い上記第１、２、３関節の各関節変位を求める。

即ち、更に具体的には、先ず、前出の（２７）式で各軸の値を算出する。第４、５、６軸の変位に関しては、（２７）式で求めた値をそのまま採用する。
次に、第４、５、６軸の変位と先端位置lnowから、図４に示す第３軸の先端位置lnowi3を算出する。
次に、（２７）式で算出した各軸の変位に対応するフラグを求める。例えば、図４の６軸垂直多関節ロボットでは、第３軸の値が０になる時も特異点であるが、フラグ切替前は第３軸が＋の値を持つフラグ（ここではフラグ３と呼ぶことにする）で、フラグ切替後は第３軸が−の値を持つフラグ（ここではフラグ４と呼ぶことにする）になる場合、（２７）式において第３軸の値が＋の場合はフラグを３とし、第３軸の値が−の場合はフラグを４とする。
フラグが３の場合は、第３軸の先端の位置lnowi3から第１、２、３軸の変位を求める際、第３軸の値が＋になる解を選択する。フラグが４の場合は、第３軸の先端の位置 lnowi3 から第１、２、３軸の変位を求める際、第３軸の値が−になる解を選択する。
実施の形態６の逆変換手段Ｃ１３において、加減速手段３で算出された指令移動量がフラグ切替直前の地点と切替地点の間にある場合のフローを図１０に示す。

実施の形態６と実施の形態１とを比べると、実施の形態６ではフラグを切り替える区間においても先端位置を本来の位置姿勢に保つことが可能になる長所がある。一方、フラグを切り替える際に、実施の形態１と比べると速度の変動が大きくなり振動を誘発する可能性があるという短所がある。
また、実施の形態６と実施の形態５とを比べると、実施の形態６では先端位置は必ず本来の位置を保てるが、先端の姿勢に関しては、フラグを切り替える区間では保てない場合がある。例えば、図４のロボットで、第５軸の値が０になる特異点近傍でフラグが切り替わる場合、実施の形態６では先端の姿勢は本来の姿勢からずれてしまう。
一方、フラグを切り替える際に実施の形態５では速度変動が大きくなる動作に対しても、実施の形態６では速度変動が大きくならない場合がある。例えば、図４のロボットで、第５軸の値が０になる特異点近傍でフラグが切り替わる場合、実施の形態５では速度変動が大きくなる可能性があるが、実施の形態６では速度変動は大きくならない。

以上のように、この発明の実施の形態６における指令値生成装置では、フラグ切替直前地点とフラグ切替地点との両地点間では、上記フラグ決定手段から入力した上記両地点での逆変換結果である各関節変位と先端位置に基づき各関節変位を一旦算出し、第１、２、３関節以外の関節変位は当該一旦算出した値に決定し、当該決定した第１、２、３関節以外の関節変位と上記先端位置から第３関節の先端位置を算出し、当該一旦算出した関節変位からその関節変位に対応するフラグを求め、上記第３関節の先端位置から当該フラグを使用して逆変換を行い上記第１、２、３関節の各関節変位を求めるようにしたので、フラグを切り替える区間においても先端位置を本来の位置姿勢に保つことが可能になるという効果がある。

実施の形態７．
この実施の形態７は、先の実施形態１での変換結果選択手段９の変形例である。実施の形態１では各軸の移動量とリミットの両方を考慮していたが、実施の形態７では各軸の移動量のみを考慮する。以下、この異なる部分を中心に説明する。
変換結果選択手段９では、再逆変換手段８で再度逆変換が実施されなかった場合、即ち、特異点近傍になかった場合は何もしない。再逆変換手段８で再度逆変換が実施された場合は逆変換の解の選択を実施する。
前回の周期の逆変換結果をtho、今回の周期の逆変換手段Ｂ７における逆変換結果をtha、再逆変換手段８での逆変換結果をthbとする。また、０以上１以下の定数waを用いて前出の（１６）式でthcを定義する。

先ず、現在のフラグをそのまま用いた場合の各軸移動量dthaを前出の（１７）式で算出する。
次に、dthaの各軸要素の少なくとも１つが規定値以上であるかどうかの判別を行う。少なくとも１つが規定値以上の場合はフラグの変更を検討する。そうでない場合はフラグは変更せず、thaを逆変換結果として採用する。
フラグの変更を検討する場合、tha、thb、thcを逆変換結果として選択した場合の各軸の移動量の絶対値の最大値maxdtha， maxdthb， maxdthcを算出する。ここでmaxdthaはdthaの各軸要素の絶対値の最大値である。次に、以下の条件１〜３の判別を順に行い、判別結果に応じたフラグ及び逆変換結果を選択する。

条件１：maxdthb<maxdthaの場合はフラグを切り替えてthbを逆変換結果として採用する。但し、thbが動作範囲外となる場合はフラグを切り替えず、逆変換結果もthaを選択する。
条件２：条件１が成立せず、maxdthc<maxdthaの場合はフラグは切り替えないがthcを逆変換結果として採用する。但し、thcが動作範囲外となる場合は、逆変換結果もthaを選択する。
条件３：条件１、２がいずれも成立しない場合はフラグを切り替えず、逆変換結果もthaを選択する。

この実施の形態７では、動作リミットは考慮しないので、フラグを切り替えないと特異点近傍で動作リミットを越え、フラグを切り替えれば動作リミット内に各関節変位が入る動作に対しては、実施の形態１では動作リミットを越えることなく動作できるのに対して、実施の形態５では動作できない点が不利になるが、変換結果選択手段９での演算処理が簡便になるという効果がある。

実施の形態８．
この実施の形態８は、先の実施形態１での変換結果選択手段９の変形例である。実施の形態１では各軸の移動量とリミットの両方を考慮していたが、実施の形態８では各軸のリミットのみを考慮する。以下、この異なる部分を中心に説明する。
変換結果選択手段９では、再逆変換手段８で再度逆変換が実施されなかった場合、即ち、特異点近傍になかった場合は何もしない。再逆変換手段８で再度逆変換が実施された場合は逆変換の解の選択を実施する。
今回の周期の逆変換手段Ｂ７における逆変換結果をtha、再逆変換手段８での逆変換結果をthbとする。

先ず、thaがリミット近傍にあるかどうかを判別する。thaがリミット近傍にない場合はフラグを切り替えずに逆変換結果としてthaを選択する。thaがリミット近傍にある場合は、thbがリミット近傍にあるかどうかを判別する。thbがリミット近傍にない場合は、フラグを切り替えてthbを逆変換の解として選択する。thbもリミット近傍にある場合はフラグを切り替えずthaを逆変換の解として選択する。

この実施の形態８では、各軸の移動量を考慮しないので、フラグを切り替えたほうが速度を低下させる必要がなく高速に動ける動作でもフラグが切り替わらないため、実施の形態１と比べると動作時間が長くなる点が不利になるが、変換結果選択手段９での演算処理が簡便になるという効果がある。

実施の形態９．
この実施の形態９は、先の実施形態１での変換結果選択手段９の変形例である。実施の形態１とは変換結果選択手段９における逆変換の解の選択のフローにおいて、ステップ３での条件判別が異なっている。
この実施の形態９におけるステップ３では、tha、thbを逆変換結果として選択した場合の各軸の移動量の絶対値の最大値maxdtha， maxdthbを算出する。ここで、maxdthaはdthaの各軸要素の絶対値の最大値である。ステップ３では、以下の条件１〜３の判別を順に行い、判別結果に応じたフラグ及び逆変換結果を選択する。
条件１：maxdthb<maxdthaの場合はフラグを切り替えてthbを逆変換結果として採用する。但し、thbが動作範囲外となる場合はフラグを切り替えず、逆変換結果もthaを選択する。
条件２：条件１が成立しない場合に、先ず、
thd = wd*thb+(1-wd)*tha （２８）
dthd=thd-tho （２９）
を算出し、dthdのすべての要素が規定値以下となる最大のwd(0≦wd≦1)であるwdmaxを算出する。dthdのすべての要素が規定値以下となるwd(0≦wd≦1)が存在しない場合はフラグを切り替えず逆変換結果もthaを採用する。dthdのすべての要素が規定値以下となるwd(0≦wd≦1)が存在する場合はフラグは切り替えないが
thdmax= wdmax*thb+(1-wdmax)*tha （３０）
を逆変換結果として採用する。但し、thdmaxが動作範囲外となる場合は、逆変換結果もthaを選択する。
条件３：条件１、２がいずれも成立しない場合はステップＵ４（実施の形態１の図６参照）へ進む。

この実施の形態９では、２つのフラグでの逆変換結果の重み付和を逆変換の解とする場合に、その重みの最適化計算を実施するもので、実施の形態１と比べると動作時間を短縮できる効果がある一方で計算量が多いという短所がある。

実施の形態１０．
この実施の形態１０は、先の実施形態１での移動速度演算手段５の変形例である。実施の形態１では、各軸の移動速度が許容値を超える場合のみ移動速度の修正を行っていたが、実施の形態１０では各軸の移動速度が許容値を超えない場合も移動速度の修正を下記のように実施する。

先ず、動作開始から2*k≦ft/stの間はrdth_kmaxが０でないときは常に
v_k=v_k-1/ rdth_kmax （３１）
で移動速度を修正し再度移動量dl_k、l_kを算出する。rdth_kmaxが０のときのみ修正しない。 移動速度・移動量演算手段１から出力される2*k-1番目の移動速度vs(2*k-1)及び2*k番目の移動速度vs(2*k)はいずれも（３１）式のv_kである。また、移動量に関しては求め直したdl_k、l_kを用いて実施の形態１と同様に（８）式、（９）式で求められる。
2*k>ft/stとなって以降は、rdth_k2maxが０でないときは常に
v_k2=v_k2-1/rdth_k2max （３２）
で移動速度を修正し再度移動量dl_k2、l_k2を算出する。rdth_k2maxが０のときのみ修正しない。
移動速度・移動量演算手段１から出力される移動速度は（３２）式のv_k2であり、移動量は再度算出したl_k2である。

先の実施の形態１では、先端速度の上限をv_ｏとしている。即ち、関節速度に余裕があっても先端速度を上げない。
一方、実施の形態１０では、関節速度に余裕がある場合は、先端速度をv_ｏよりも大きな値に設定する。従って、実施の形態１０の方が高速に動作できる。
しかし、計算量の観点から見ると、実施の形態１では関節速度が許容値を超える場合のみ同一の周期で逆変換手段Ｂ７での逆変換を２度（それ以外はｌ度）実施するが、実施の形態１０では常に同一の周期で逆変換手段Ｂ７での逆変換を２度実施することになる。言い換えれば、実施の形態１では、移動速度・移動量演算手段１の内部のブロック要素５〜９のループが１周期にｌ回（上限を超える場合のみ２回）であるのに対して、実施の形態１０では、移動速度・移動量演算手段１の内部のブロック要素５〜９のループがｌ周期に必ず２回実施する。そのため、実施の形態１０の方が計算量が多くなる。

以上のように、この発明の実施の形態１０における指令値生成装置では、その移動速度演算手段は、上記第２の逆変換手段からの各関節変位から各関節速度を演算し、上記各関節速度と当該各関節で定められた所定の最大許容値との比に応じて上記先端移動速度を修正して出力するので、先端速度をより高速に動作させ得るという効果がある。

実施の形態１１．
この実施の形態１１は、実施の形態１とは、移動速度演算手段５、移動量演算手段６及び移動速度修正手段３での動作が異なっている。
先ず、移動速度演算手段５では動作開始から2*k≦ft/stの間も1周期分の計算を実施する。即ち、動作開始から終了まで全く同じ動作を行う。
動作開始時には、移動速度の初期値を規定値であるv₀とし、k回目の周期における移動速度の初期値をv_k-1とする。ここでk=1のときはv_k-1は規定値v₀とする。次にst秒での移動量
dl_k= v_k-1*st （３３）
を移動量算出手段６で算出し、さらにdl_kの累積値l_kを算出し動作開始地点からl_kだけ進んだ地点p_kを移動量算出手段６で算出する。

次に、逆変換手段Ｂ７でp_kを逆変換し、必要に応じて再逆変換手段８での再逆変換、及び変換結果選択手段９で複数ある逆変換の解の選択を行うことによりp_kを逆変換した結果であるth_kを算出する。
逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作については実施の形態１と同一である。
th_kは、移動速度演算手段５にフィードバックされ、前回の演算における逆変換結果th_k-1から各軸毎の移動量
dth_k = th_k−th_k-1 （３４）
の計算を移動速度演算手段５で実施する。更に、dth_kの各軸成分とそれぞれの許容最大値dthmaxとの比
rdth_k(i)＝|dth_k(i)|/(dthmax(i)) （３５）
を算出しrdth_k(i)の最大値rdth_kmaxを求める。

rdth_kmax≦1の場合は移動速度、移動量として1回目の計算結果をそのまま採用し、k番目の移動速度vs(k)をv_k-1とする。また、k番目の移動量ls(k)をl_kとする。
また、v_k-1=v₀の場合は
v_k=v₀ （３６）
とする。v_k-1<v₀の場合は
v_k =min(v_k-1/rdth_kmax， v₀ ) （３７）
とする。

rdth_kmax＞1の場合は、移動速度を v_k-1/rdth_kmaxに修正し、dl_k及びl_kを算出し直す。k番目の移動速度vs(k)をv_k-1/rdth_kmaxとする。また、移動量に関しては求め直したl_kとする。また、
v_k =v_k-1/rdth_kmax （３８）
とする。

移動速度修正手段３では、動作開始からの演算回数kが2*filaを超えるまではｋが偶数になる度に、
vlmin(k/2)＝min(vs(l)，vs(2)・・，vs(k))とする。
演算回数kが2^＊filaを超えてからは、
v1min(k-fila)＝min(vs(k-2^＊fila+1 )，vs(k-2^＊fila+2)・・ ，vs(k)) （３９）
で算出する。

実施の形態１１では、移動速度演算手段５で常に１周期分しか計算しないため、実施の形態１と比べると処理が簡単になり計算時間が削減できる長所がある。しかし、vlmin()がfilaだけ遅れるため動作時間が長くなるという問題があるが、その他の面については、以上で説明した各実施の形態での効果を奏する。

実施の形態１２．
図１１は、この発明の実施の形態１２における指令値生成装置を示すブロック図である。この実施の形態１２は、先の実施の形態２とは、切替地点記憶手段１０、フラグ決定手段１１、補正手段１２がないことと、逆変換手段Ａ４の代わりに逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９を用いることが異なっている。また、変換結果選択手段９でフラグを切り替えてもフラグ切替地点及びその直前の地点の情報を記憶することも実施しない。
加減速手段３の後に実施する逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作は、移動速度・移動量演算手段１内部での逆変換手段Ｂ７、再逆変換手段８、変換結果選択手段９の動作と同一である。

この実施の形態１２では、加減速後のフラグの切替(thcを選択する場合も含めて）が１周期で必ず完了するため加速度変化が実施の形態１と比べて大きくなる場合があり、そのために振動を誘発する可能性がある短所はあるが、構成が簡単になり、その他の面では、以上で説明した各実施の形態での効果を奏する。

実施の形態１３．
図１２は、この発明の実施の形態１３における指令値生成装置を示すブロック図である。この実施の形態１３は、先の実施の形態１２とは、再逆変換手段８、変換結果選択手段９がないことが異なっている。即ち、特異点近傍にない場合の実施の形態１２と全く同じ動作をする。

この実施の形態１３では、構成が簡単となり、特異点近傍を通過しない場合は（フラグの切り替えは実施の形態１、１２でも起こらないため）、実施の形態１、１２と同じ動作をする。その他の面では、以上で説明した各実施の形態での効果を奏する。

実施の形態１４．
この実施の形態１４は、先の実施の形態１の加減速手段３の変形例である。実施の形態１とは、加減速手段３で用いるフィルタが1段の移動平均フィルタと1段の1次遅れフィルタから構成され、フィルタの窓長の総和として1段の移動平均フィルタの窓長を用いることが異なっている。

この実施の形態１４では、２段目のフィルタを移動平均フィルタではなく1次遅れフィルタとしているため、記憶容量が少なくてすむ長所がある。一方、１次遅れフィルタでは移動平均フィルタのように窓長が正確には定められないため、移動速度修正手段２で算出する最小値が加減速の影響を正確に考慮することができず、関節速度が許容値を超える、もしくは速度を必要以上に落とす必要がある可能性があるという短所があるが、その他の面では、以上で説明した各実施の形態での効果を奏する。

実施の形態１５．
図１３は、この発明の実施の形態１５における指令値生成装置を示すブロック図である。実施の形態１５では、先の図４に示す6軸多関節ロボットにおける先端位置指令生成手段１４からの先端位置指令に対して各軸位置指令を生成する。
先ず、５軸／６軸切替手段１５では、６軸ロボットを動作させるプログラム内に記載されているコマンドに基づいて５軸ロボットとして動作させるか、６軸ロボットとして動作させるかが指定される。先端位置指令生成手段１４では、指令生成周期ごとの、６軸多関節ロボットの先端位置指令を生成する。先端位置指令は、加減速区間で加速度が一定となる先端速度台形指令を積分することにより得られている。

逆変換手段１８には、５軸／６軸切替手段１５から出力される５軸、６軸の指定と、先端位置指令生成手段１４で生成された先端位置指令が入力される。５軸が指定されている場合は第4軸の値を常時、所定の値、例えば、０とし、第４軸が固定された５軸ロボットとして逆変換を実施する５軸逆変換手段１６が選択され、６軸が指定されている場合は、６軸ロボットのまま逆変換を実施する６軸逆変換手段１７が選択され、選択された逆変換手段で逆変換が行われ、各軸の位置指令が算出される。

6軸ロボットから５軸ロボットへ指定が切り替わった場合、即ち、６軸ロボットとして動作中にプログラム内に記載されているコマンドに基づいて５軸ロボットとして動作させることが指定されると、５軸ロボットとして動作させることを指定するコマンドが処理された地点から第４軸の値が０で残りの軸の値はコマンドが処理された地点への動作命令が自動的に挿入される。

この実施の形態１５は、６軸ロボットではあるが、５軸ロボット（世の中の大多数の５軸ロボットは６軸ロボットの第４軸の関節変位が、例えば、０に固定されているものに相当する）として動作できる動作に対しては、６軸ロボットであるにもかかわらず５軸ロボットとして動作させることにより、６軸ロボットで最も問題となる第５軸の変位が０である特異点もしくはその近傍を通過する動作においても、先端速度を落とすことなく動作させるものである。５軸ロボットで第５軸の変位が０である特異点を通過できるのは、第４軸の変位が０に固定されているため、第６軸の変位が一意に定まり、５軸ロボットでは特異点にならないためである。

換言すると、この実施の形態１５に係る発明は、その適用は上述した６軸ロボットになるメカニカルシステムに限られるものの、特異点もしくはその近傍を通過する動作においてその先端速度を落とすことなく円滑に動作させることを意図したものであり、その意味で、先に説明した実施の形態１等に係る発明と課題を同一とするものである。

以上でロボットに適用した場合について説明したが、各関節の駆動軸を制御することにより、その先端を所定の軌跡に沿って移動させるものであれば、たとえロボットと呼称されていないメカニカルシステムにも、本願発明は同様に適用出来、同等の効果を奏するものである。

この発明の実施の形態１における指令値生成装置を示すブロック図である。 移動速度・移動量演算手段１の動作を説明するためのフローチャートである。 図２に連なるフローチャートである。 ６軸多関節ロボットの構成を示す図である。 特異点と一致する場合に、逆変換結果を生成する逆変換結果生成手段の動作を説明するためのフローチャートである。 変換結果選択手段９の動作を説明するためのフローチャートである。 移動速度修正手段３の動作を説明するためのフローチャートである。 この発明の実施の形態４における指令値生成装置を示すブロック図である。 この発明の実施の形態５における逆変換手段Ｃ１３の動作を説明するためのフローチャートである。 この発明の実施の形態６における逆変換手段Ｃ１３の動作を説明するためのフローチャートである。 この発明の実施の形態１２における指令値生成装置を示すブロック図である。 この発明の実施の形態１３における指令値生成装置を示すブロック図である。 この発明の実施の形態１５における指令値生成装置を示すブロック図である。

符号の説明

１ 移動速度・移動量演算手段、２ 移動速度修正手段、３ 加減速手段、
４ 逆変換手段Ａ、５ 移動速度演算手段、６ 移動量演算手段、７ 逆変換手段Ｂ、
８ 再逆変換手段、９ 変換結果選択手段、１０ 切替地点記憶手段、
１１ フラグ決定手段、１２ 補正手段、１３ 逆変換手段Ｃ、
１４ 先端位置指令生成手段、１５ ５軸／６軸切替手段、１６ ５軸逆変換手段、
１７ ６軸逆変換手段、１８ 逆変換手段。

Claims (14)

1. 複数の関節を備えたメカニカルシステムの先端を、上記各関節の駆動軸を制御することにより、予め定められた軌跡に沿って移動させるものにおける、上記各関節の駆動軸を制御するための指令値を生成する指令値生成装置であって、
   上記各関節の駆動軸の速度制限を満足するように、上記先端の移動速度およびこの移動速度を積分して得られる上記軌跡に沿った上記先端の移動量を演算する移動速度・移動量演算手段、この移動速度・移動量演算手段における任意の演算時点ｔで演算された移動速度（ｔ）および移動量（ｔ）に対し、上記演算時点ｔを含む所定演算時間範囲内または上記移動量（ｔ）を含む所定演算距離範囲内での移動速度の演算結果に上記移動速度（ｔ）より小さい移動速度が存在しない場合は上記移動速度（ｔ）を踏襲し、上記移動速度（ｔ）より小さい移動速度が存在する場合は上記移動速度（ｔ）を当該小さい移動速度に置換した修正移動速度を作成し、この修正移動速度を積分して得られる移動量が上記移動量（ｔ）に達するまで出力する移動速度修正手段、上記各関節の駆動軸の加減速駆動特性を円滑化するため上記移動速度修正手段からの修正移動速度に波形処理として移動平均フィルタ処理を施し先端移動速度指令として出力する加減速手段、およびこの加減速手段からの上記先端移動速度指令を積分して得られる先端移動量指令を実現する上記各関節の変位指令を算出（以下、逆変換と称す）し上記変位指令から各関節の駆動軸を制御するための位置指令値または速度指令値を生成する第１の逆変換手段を備えたことを特徴とする指令値生成装置。
2. 上記移動速度修正手段において、上記移動速度（ｔ）に替わって上記修正移動速度として採用する上記当該小さい移動速度として、上記所定演算時間範囲内または所定演算距離範囲内での移動速度の演算結果の内の最小値を採用することを特徴とする請求項１に記載の指令値生成装置。
3. 上記移動速度修正手段において、上記移動速度（ｔ）に替わって上記修正移動速度として採用する上記当該小さい移動速度として、上記所定演算時間範囲内または所定演算距離範囲内での移動速度の演算結果の内の最新極小値を採用することを特徴とする請求項１に記載の指令値生成装置。
4. 上記加減速手段として移動平均フィルタを使用し、上記所定演算時間範囲を上記移動平均フィルタの窓長の総和に設定したことを特徴とする請求項１ないし３のいずれかに記載の指令値生成装置。
5. 上記移動速度・移動量演算手段は、上記先端移動速度を演算する移動速度演算手段、上記演算移動速度から上記先端移動量を演算する移動量演算手段、および上記先端移動量から上記各関節変位を演算する第２の逆変換手段を備え、
   上記移動速度演算手段は、上記第２の逆変換手段からの各関節変位から各関節速度を演算し、上記各関節速度と当該各関節で定められた所定の最大許容値との比の最大値を算出し、当該最大値が１より大きい場合は上記先端移動速度を当該最大値で除算した値に修正して出力することを特徴とする請求項１ないし４のいずれかに記載の指令値生成装置。
6. 上記移動速度・移動量演算手段は、更に、上記第２の逆変換手段での逆変換結果における、上記各関節のいずれかの変位の時間変化量が所定の既定値を越える特異点近傍にあるかどうかの判断を行い、上記特異点の近傍にある場合上記第２の逆変換手段で用いたフラグと当該特異点に関して対になるフラグで再度逆変換を行う再逆変換手段、および上記第２の逆変換手段による逆変換結果を採用した場合の上記各関節変位の最大値と上記再逆変換手段による逆変換結果を採用した場合の上記各関節変位の最大値とを比較し、上記再逆変換手段による逆変換結果を採用した場合の上記各関節変位の最大値の方が小さい場合に上記再逆変換手段による逆変換結果を選択する変換結果選択手段を備えたことを特徴とする請求項５記載の指令値生成装置。
7. 上記移動速度・移動量演算手段は、更に、上記第２の逆変換手段での逆変換結果における、上記各関節のいずれかの変位の時間変化量が所定の既定値を越える特異点近傍にあるかどうかの判断を行い、上記特異点の近傍にある場合上記第２の逆変換手段で用いたフラグと当該特異点に関して対になるフラグで再度逆変換を行う再逆変換手段、および上記第２の逆変換手段による前回の演算ステップの逆変換結果と今回の演算ステップの逆変換結果との差が所定の規定値以上のとき、上記第２の逆変換手段による逆変換結果と、上記再逆変換手段による逆変換結果と、上記両逆変換結果を重み付けして加算した逆変換結果との３者の逆変換結果のいずれかを選択する変換結果選択手段を備え、
   上記変換結果選択手段は、上記第２の逆変換手段での逆変換結果をtha、上記再逆変換手段での逆変換結果をthb、０以上１以下の定数waを用い、式thc=wa*thb+(1-wa)*thaで算出される重み付けして加算された逆変換結果をthcとしたとき、逆変換結果としてtha、thb、thcをそれぞれ選択した場合の上記各関節変位の最大値maxdtha、maxdthb、maxdthcを算出し、maxdthb<maxdthaが成り立つ場合はthbを逆変換結果として選択し、maxdthb≧maxdtha、かつmaxdthc<maxdthaが成り立つ場合はthcを逆変換結果として選択するようにしたことを特徴とする請求項５記載の指令値生成装置。
8. 上記再逆変換手段は、上記第２の逆変換手段による逆変換結果による上記各関節変位が＋側リミットからある定数を減算した値以上、もしくは、−側リミットに同じ定数を加算した値以下となる範囲にある所定のリミット近傍にあるときも逆変換を行うようにし、
   上記変換結果選択手段は、上記第２の逆変換手段による逆変換結果が上記各関節変位の所定のリミット近傍以内にあり、上記再逆変換手段による逆変換結果が上記各関節変位の所定のリミット近傍以内にないとき、上記再逆変換手段による逆変換結果を選択するようにしたことを特徴とする請求項６または７に記載の指令値生成装置。
9. 上記変換結果選択手段の選択結果に伴い上記フラグが切り替わった地点とその直前地点である上記変換結果選択手段でのフラグ切替の１つ前の演算ステップでの地点との両地点を記憶する切替地点記憶手段、この切替地点記憶手段に記憶された情報を入力し上記第１の逆変換手段での逆変換に使用するフラグを決定するフラグ決定手段、および上記加減速手段の出力から求めた先端位置が上記両地点の間にある場合、上記フラグ決定手段から入力した、フラグ切替直前の地点の移動量lfb、逆変換結果thfbとフラグ切替地点における移動量lf、逆変換結果thfを入力し、更に、上記加減速手段で算出された指令移動量lnowを用いて表される下式に基づき上記第１の逆変換手段からの各関節変位を補正する補正手段を備えたことを特徴とする請求項６ないし８のいずれかに記載の指令値生成装置。
   (thf-thfb)*(lnow-lfb)/(lf-lfb)+thfb
10. 上記変換結果選択手段の選択結果に伴い上記フラグが切り替わった地点とその直前地点である上記変換結果選択手段でのフラグ切替の１つ前の演算ステップでの地点との両地点を記憶する切替地点記憶手段、およびこの切替地点記憶手段に記憶された情報を入力し上記第１の逆変換手段での逆変換に使用するフラグを決定するフラグ決定手段を備え、
    上記第１の逆変換手段は、上記加減速手段の出力から求めた先端位置が、上記直前地点に達するまでは切替前のフラグで逆変換を行い、上記切替地点を通過後は切替後のフラグで逆変換を行い、上記両地点間では、上記先端位置が定められた軌跡を維持するように、当該切替で逆変換結果である関節変位が変化する関節の関節変位に基づき他の関節の関節変位を求めるようにしたことを特徴とする請求項６ないし８のいずれかに記載の指令値生成装置。
11. 上記変換結果選択手段の選択結果に伴い上記フラグが切り替わった地点とその直前地点である上記変換結果選択手段でのフラグ切替の１つ前の演算ステップでの地点との両地点を記憶する切替地点記憶手段、およびこの切替地点記憶手段に記憶された情報を入力し上記第１の逆変換手段での逆変換に使用するフラグを決定するフラグ決定手段を備え、
    上記第１の逆変換手段は、上記加減速手段の出力から求めた先端位置が、上記直前地点に達するまでは切替前のフラグで逆変換を行い、上記切替地点を通過後は切替後のフラグで逆変換を行い、上記両地点間では、上記フラグ決定手段から入力した上記両地点での逆変換結果である各関節変位と先端位置に基づき各関節変位を一旦算出し、当該一旦算出した関節変位からその関節変位に対応するフラグを求め、上記先端位置から当該フラグを使用して逆変換を行い各関節変位を求めるようにしたことを特徴とする請求項６ないし８のいずれかに記載の指令値生成装置。
12. ５または６関節のメカニカルシステムにおいて、上記変換結果選択手段の選択結果に伴い上記フラグが切り替わった地点とその直前地点である上記変換結果選択手段でのフラグ切替の１つ前の演算ステップでの地点との両地点を記憶する切替地点記憶手段、およびこの切替地点記憶手段に記憶された情報を入力し上記第１の逆変換手段での逆変換に使用するフラグを決定するフラグ決定手段を備え、
    上記第１の逆変換手段は、上記加減速手段の出力から求めた先端位置が、上記直前地点に達するまでは切替前のフラグで逆変換を行い、上記切替地点を通過後は切替後のフラグで逆変換を行い、上記両地点間では、上記フラグ決定手段から入力した上記両地点での逆変換結果である各関節変位と先端位置に基づき各関節変位を一旦算出し、第１、２、３関節以外の関節変位は当該一旦算出した値に決定し、当該決定した第１、２、３関節以外の関節変位と上記先端位置から第３関節の先端位置を算出し、当該一旦算出した関節変位からその関節変位に対応するフラグを求め、上記第３関節の先端位置から当該フラグを使用して逆変換を行い上記第１、２、３関節の各関節変位を求めるようにしたことを特徴とする請求項６ないし８のいずれかに記載の指令値生成装置。
13. 上記移動速度・移動量演算手段は、動作開始からの所定の期間内のみ、移動速度および上記各関節で定められた最大許容値の速度を本来の値より大きく設定して演算するようにしたことを特徴とする請求項１ないし１２のいずれかに記載の指令値生成装置。
14. 上記逆変換手段による逆変換結果が一意に定まらない関節が生じる特異点にあるとき、前後の演算ステップにおける上記逆変換手段による逆変換結果に基づき逆変換結果を定める逆変換結果生成手段を備え、
    上記逆変換結果生成手段は、前回および前々回の演算ステップの逆変換結果から前回移動量を算出し、前回の演算ステップの逆変換結果に上記前回移動量を加算することにより当該一意に定まらない関節の逆変換結果を一旦求め、この一旦求めた逆変換結果を使用して算出した上記先端の位置姿勢と逆変換前における上記先端の位置姿勢との差に基づき上記一旦求めた逆変換結果を補正した値を当該一意に定まらない関節の逆変換結果として出力することを特徴とする請求項１ないし１３のいずれかに記載の指令値生成装置。

Images