以下、垂直多関節型ロボットに具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。
はじめに、ロボット10の概要を図1に基づいて説明する。
同図に示すように、ロボット10は、各関節の回転中心軸線として、第1軸線J1、第2軸線J2、第3軸線J3、第4軸線J4、第5軸線J5、第6軸線J6を有する6軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、モータ駆動により原点位置を基準として各部が動作する。なお、各関節には、各軸線を中心として各部を滑らかに回転可能とする軸受け(軸受け手段)が設けられている。
ロボット10は、床に設置されており、第1軸線J1が鉛直方向へ延びている。ロボット10において、基台11は、床等に固定される固定部12と、その固定部12の上方に設けられる回転部13(第1回転部)とを有しており、回転部13が第1軸線J1を回転中心として水平方向に回転可能になっている。すなわち、回転部13は、第1軸線J1の方向に延びるとともに、固定部12により第1軸線J1を中心として回転可能に支持されている。
下アーム15(第2回転部)が、水平方向に延びる第2軸線J2を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム15は、第1軸線J1に直交する平面に含まれる第2軸線J2から離れる方向へ延びるとともに、回転部13により第2軸線J2を中心として回転可能に支持されている。下アーム15は、基本姿勢として鉛直方向に延びる向きに設けられている。
下アーム15の上端部には、上アーム16が、水平方向に延びる第3軸線J3を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム16は、第2軸線J2に平行な第3軸線J3から離れる方向へ延びるとともに、下アーム15により第3軸線J3を中心として回転可能に支持されている。上アーム16は、基本姿勢として水平方向に延びる向きに設けられている。
上アーム16は、基端側と先端側とで2つのアーム部に分割されて構成されており、基端側は第1上アーム16A(第3回転部)、先端側は第2上アーム16B(第4回転部)となっている。第2上アーム16Bは、上アーム16の長手方向に延びる第4軸線J4を回転中心として、第1上アーム16Aに対してねじり方向に回転可能になっている。すなわち、第2上アーム16Bは、第3軸線J3に直交する平面に含まれる第4軸線J4の方向に延びるとともに、第1上アーム16Aにより第4軸線J4を中心として回転可能に支持されている。
上アーム16(詳しくは第2上アーム16B)の先端部には、手首部17(第5回転部)が設けられている。手首部17は、水平方向に延びる第5軸線J5を回転中心として、第2上アーム16Bに対して回転可能になっている。すなわち、手首部17は、第4軸線J4に直交する第5軸線J5から離れる方向へ延びるとともに、第2上アーム16Bにより第5軸線J5を中心として回転可能に支持されている。
手首部17の先端部には、ワークやツール等を取り付けるためのハンド部18(第6回転部)が設けられている。ハンド部18は、その中心線である第6軸線J6を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部18は、第5軸線J5に直交する第6軸線J6の方向に延びるとともに、手首部17により第6軸線J6を中心として回転可能に支持されている。
図2に、ロボット10の制御装置20及びその周辺構成を示す。
制御装置20は、ロボット10の動作を制御するものであり、CPU21、ROM22、RAM23、駆動回路25、及び位置検出回路26等を備えている。これらは互いに電気的に接続されている。ROM22は、ロボット10のシステムプログラムや動作プログラム等を記憶している。RAM23は、これらのプログラムを実行する際にパラメータの値等を記憶する。CPU21は、これらのROM22やRAM23の記憶内容に基づいて、各種プログラムを実行することにより、ロボット10の動作を制御する。
ロボット10において、各軸線J1〜J6を回転中心軸とした各関節には、それぞれ前段部側にサーボモータ31が設けられている。CPU21は、駆動回路25を通じて、各サーボモータ31の駆動を行う。これにより、各関節において、前段部に設けられたサーボモータ31の駆動により、後段部がそれぞれ回転動作する。
各サーボモータ31には、その出力軸を制動する非励磁作動型の電磁ブレーキ33と、出力軸の回転角度(回転位置)に応じたパルス信号を出力するエンコーダ32とがそれぞれ設けられている。
各電磁ブレーキ33は、ばね等の弾性力に基づきサーボモータ31の出力軸の制動を行い、励磁コイルへの電力供給に基づき出力軸の制動を解除する。CPU21は、駆動回路25を通じて、各電磁ブレーキ33の駆動を行う。CPU21は、各電磁ブレーキ33による出力軸の制動を解除した状態において各サーボモータ31の駆動を行う。
各エンコーダ32は、所定パターンに形成された回転子の回転を磁気的又は光学的に検出する検出素子と、その検出素子の信号を処理するICとを有している。位置検出回路26には、各エンコーダ32の検出信号がそれぞれ入力される。位置検出回路26は、各エンコーダ32の検出信号に基づいて、回転部13、下アーム15、第1上アーム16A、第2上アーム16B、手首部17、及びハンド部18の各位置(各角度)を検出する。
具体的には、位置検出回路26は、固定部12に対する回転部13の回転角度(回転位置)、回転部13に対する下アーム15の回転角度、下アーム15に対する第1上アーム16Aの回転角度、第1上アーム16Aに対する第2上アーム16Bの回転角度、第2上アーム16Bに対する手首部17の回転角度、手首部17に対するハンド部18の回転角度をそれぞれ検出し、それら検出した位置情報をCPU21に出力する。位置検出回路26は、上記の各回転角度を0.1°単位で検出可能である。
そして、CPU21は、動作プログラムを実行することにより、位置検出回路26から入力される位置情報に基づいて、ロボット10の上記各部の動作をフィードバック制御する。また、CPU21は、後述する修正変動パターン設定制御を実行する。
(ティーチング)
修正変動パターン設定制御に先立って、ロボット10に対して動作(アームを移動させる軌跡)のティーチングを行う。このティーチングでは、作業者が、ロボット10に作業を行わせるべく、ロボット10のハンド部18を移動させる各動作点(各移動先)を教示する。さらに、作業者は、ハンド部18を移動させる際の軌跡の形状、上限速度等を設定する。制御装置20は、この教示内容及び設定に従って速度変動パターン(各種パラメータ)を作成し、その作成した速度変動パターンによりロボット10を動作させる。作業者は、このロボット10の動作結果を評価して、必要に応じて上記設定を変更する。
図3〜5を参照して、ロボットの2つの回転部を例として、各回転部の速度変動パターンを設定する手順について説明する。なお、図3(a)は第1回転部の目標回転角度を示しており、図3(b)はそれに対する第1回転部の速度変動パターンを示している。図4(a)は第2回転部の目標回転角度を示しており、図4(b)はそれに対する第2回転部の速度変動パターンを示している。図5(a)は主回転部の速度変動パターンを示しており、図5(b)はそれ以外の回転部の速度変動パターンを設定する態様を示している。
まず、図3(a),図4(a)に示すように、各回転部の目標回転角度を設定する。ここでは、各回転部を原点(0)から回転させる場合を考え、第1回転部の目標回転角度を回転角度θ1とし、第2回転部の目標回転角度を回転角度θ2とする。回転角度θ1は、回転角度θ2よりも大きく設定されている(θ1>θ2)。
ロボット10では、各回転部の回転角度を変更する場合について、標準的な速度変動パターンが設定されており、この情報が制御装置20のROM22に記憶されている。このため、CPU21は、この情報に基づいて、各回転部の目標回転角度に対して速度変動パターンをそれぞれ設定する。このようにして設定した各回転部の速度変動パターンを、図3(b),図4(b)にそれぞれ示す。
図3(b)に示すように、第1回転部では、速度V1まで加速時間Ta1で加速した後、速度V1で等速時間Tc1移動し、速度0まで減速時間Td1で減速している。また、図4(b)に示すように、第2回転部では、速度V2まで加速時間Ta2で加速した後、速度V2で等速時間Tc2移動し、速度0まで減速時間Td2で減速している。第1回転部が回転を開始してから回転を終了するまでの時間(時間Ta1+時間Tc1+時間Td1)は、第2回転部が回転を開始してから回転を終了するまでの時間(時間Ta2+時間Tc2+時間Td2)よりも長くなっている。
目標回転角度までの回転に最も時間のかかる第1回転部を主回転部に設定し、図5に示すように、この主回転部の速度変動パターンに基づいて、その他の回転部の速度変動パターンを設定する。すなわち、各回転部の回転開始から回転終了までの時間が等しくなるように、主回転部以外の回転部の速度変動パターンを補正する。詳しくは、図5(b)に示すように、第2回転部の回転角度である台形の面積を変化させないようにしつつ、第2回転部の回転開始から回転終了までの時間(時間Tam+時間Tcm+時間Tdm)を、第1回転部の回転開始から回転終了までの時間(時間Ta1+時間Tc1+時間Td1)に合わせる。なお、各回転部において、加速時間、等速時間、減速時間が互いに等しくなるようにしている(時間Ta1=時間Tam、時間Tc1=時間Tcm、時間Td1=時間Tdm)。
このようにして、教示された複数の動作点に対して、ハンド部18を各動作点間(アームの第1姿勢と第2姿勢との間)で移動させる際の速度変動パターンを設定する。例えば、第1姿勢を動作開始点とし、第2姿勢を動作終了点とする。そして、制御装置20は、各動作点間の速度変動パターンを設定した後、それらの速度変動パターンに従ってロボット10を実際に駆動し、その時の各回転部の回転角度を検出する。詳しくは、CPU21は、各速度変動パターンに基づいて各サーボモータ31を駆動する電流を算出し、駆動回路25を通じて各サーボモータ31の駆動を行う。この駆動中において、所定周期毎に、各エンコーダ32の検出信号が位置検出回路26に入力され、位置検出回路26は各エンコーダ32の検出信号に基づいて各回転部の回転角度を検出する。CPU21は、位置検出回路26から各回転部の回転角度を入力して、各回転部の回転角度連続値を検出する。
CPU21は、このように検出された各回転部の回転角度連続値に基づいて、各回転部の速度連続値を算出する。すなわち、回転角度の変化から、その時々の回転速度を算出する。さらに、CPU21は、算出された各回転部の速度連続値に基づいて、各回転部の加速度連続値を算出する。すなわち、回転速度の変化から、その時々の回転加速度を算出する。また、CPU21は、ロボット10を実際に駆動する際の各サーボモータ31の電流、すなわち上記のように各速度変動パターンに基づいて算出された各サーボモータ31の電流を、電流連続値として検出する。そして、検出された各連続値をRAM23に記憶させる。なお、各サーボモータ31において、速度、加速度、及び電流の各連続値を検出する処理が、連続値検出手段としての処理に相当する。
次に、図6のフローチャートを参照して、ロボット10をより高速で動作させるための変動パターンを設定する制御の処理手順について説明する。この一連の処理は、制御装置20のCPU21によって実行される。
まず、ロボット10の複数の動作点間のうち最適化する動作点間(最適化区間)について、各種パラメータの連続値を取得する(S11)。具体的には、上述したようにロボット10のティーチングにおいて検出した速度連続値、加速度連続値、及び電流連続値について、最適化区間での連続値をそれぞれ取得する。詳しくは、RAM23からそれらの連続値のデータを読み込む。なお、最適化区間は、ロボット10の動作の精度がそれほど要求されず、動作の速度が要求される動作区間をユーザが指定することにより決定される。
その後、カウンタ初期化処理として、前側カウンタCfrtの値を0にするとともに、後側カウンタCbakの値を最大値Cmaxにする(S12)。最大値Cmaxは、最適化する動作点間を分割する分割数である。前側カウンタCfrtは、ロボット10の第1姿勢(最適化区間の一方の端部)から所定の期間Ts毎に各種パラメータを算出する際の位置を表すカウンタである。後側カウンタCbakは、ロボット10の第2姿勢(最適化区間の他方の端部)から所定の期間Ts毎に各種パラメータを算出する際の位置を表すカウンタである。
続いて、前側カウンタCfrtに対応する位置において、ロボット10の各サーボモータ31について、定格の最大電流Imax(上限値)に対する駆動時の電流Iの電流比Iratを算出する(S13)。J番目のサーボモータ31を添え字Jで表し、前側カウンタCfrtに対応する位置を添え字Cfrtで表すと、電流比Irat[J]=|電流I[J][Cfrt]|/最大電流Imax[J]となる。
図8に示すように、最適化区間において、速度連続値(図8(a)参照)、加速度連続値(図8(b)参照)、電流連続値(図8(c)参照)がそれぞれ取得されている。速度連続値では、時間tに比例して速度Vが上昇し、定格の最大速度Vmax(上限値)に達した時点で最大速度Vmaxが維持され、その後に時間に比例して速度Vが0まで低下している。加速度連続値では、速度Vが最大速度Vmaxに到達するまで一定の加速度に維持され、速度Vが最大速度Vmaxに達した時点で加速度Aが0にされ、その後に速度Vが0に低下するまで一定の加速度(減速度)に維持されている。電流連続値では、所定の正の電流から時間tに比例して電流Iが増加し、速度Vが最大速度Vmaxに達した時点で電流Iが減少され、その後に所定の負の電流から時間tに比例して電流Iが増加している。そして、図8(c)の枠で囲んだ部分を拡大して図8(d)に示すように、前側カウンタCfrtに対応する位置において、最大電流Imaxに対する駆動時の電流Iの電流比Iratを算出する。
図6に戻り、各サーボモータ31の電流比Irat[J]のうち、最大値である最大電流比Iratmaxを選択する(S14)。
そして、最大電流比Iratmaxの逆数に基づいて、各サーボモータ31の対応する加速度Aを修正した修正加速度Aamdを算出する(S15)。ここで、電流比Iratの逆数は、電流Iを定格の最大電流Imaxまで大きくする際の倍率であり、各サーボモータ31では電流Iをこの電流比Iratの逆数倍まで大きくすることができる。上記最大電流比Iratmaxは、定格の最大電流Imaxまでの余裕が最も少ないサーボモータ31での電流比Iratとなる。このため、このサーボモータ31では、電流Iに電流比Iratの逆数を掛けると定格の最大電流Imaxとなる。
したがって、前側カウンタCfrtに対応する位置において、各サーボモータ31の加速度Aをそれぞれ最大電流比Iratmaxで割って修正加速度Aamdを算出する。すなわち、J番目のサーボモータ31を添え字Jで表し、前側カウンタCfrtに対応する位置を添え字Cfrtで表すと、修正加速度Aamd[J][Cfrt]=加速度A[J][Cfrt]/最大電流比Iratmaxとなる。この修正加速度Aamd[J][Cfrt]は、各サーボモータ31の駆動が同時に終了するように、各サーボモータの加速度A[J][Cfrt]を同じ倍率(最大電流比Iratmaxの逆数)で大きくしたものである。このとき、各サーボモータ31の加速度Aは、各サーボモータ31を駆動する電流Iに概ね比例するとみなしている。なお、最大電流比Iratmaxが0である場合には、各サーボモータの加速度A[J][Cfrt]を0とする。
続いて、修正加速度Aamdとその修正加速度Aamdに対応する各サーボモータ31の速度に基づいて、各サーボモータ31における所定の期間Ts後の速度である修正速度Vamdを算出する(S16)。詳しくは、前側カウンタCfrtに対応する位置において、ロボット10の各サーボモータ31について、前回の修正速度Vamd[J][Cfrt−1]に前回の修正加速度Aamd[J][cfrt−1]と所定の期間Tsとの積を加えて、修正速度Vamd[J][Cfrt]を算出する。すなわち、修正速度Vamd[J][Cfrt]=修正速度Vamd[J][Cfrt−1]+Aamd[J][cfrt−1]×期間Tsとなる。なお、前側カウンタCfrtの値が0の場合には、修正速度Vamd[J][0]=速度V[J][0]とする。
そして、修正速度Vamdが定格の最大速度Vmaxよりも小さくないと判定した場合には(S17:NO)、修正速度Vamdを最大速度Vmaxとする(S19)。詳しくは、前側カウンタCfrtに対応する位置において、ロボット10の各サーボモータ31について、修正速度Vamd[J][Cfrt]が定格の最大速度Vmax[J]よりも小さくないと判定した場合には、修正速度Vamd[J][Cfrt]を最大速度Vmax[J]とする。
一方、修正速度Vamdが定格の最大速度Vmaxよりも小さいと判定した場合には(S17:YES)、修正速度Vamdとその修正速度Vamdに対応する各サーボモータ31の回転角度に基づいて、各サーボモータ31における所定の期間Ts後の回転角度である修正回転角度θamdを算出する(S18)。詳しくは、前側カウンタCfrtに対応する位置において、ロボット10の各サーボモータ31について、前回の修正回転角度θamd[J][Cfrt−1]に前回の修正速度Vamd[J][cfrt−1]と所定の期間Tsとの積を加えて、修正回転角度θamd[J][Cfrt]を算出する。すなわち、修正回転角度θamd[J][Cfrt]=修正回転角度θamd[J][Cfrt−1]+Vamd[J][cfrt−1]×期間Tsとなる。なお、前側カウンタCfrtの値が0の場合には、修正回転角度θamd[J][0]=回転角度θ[J][0]とする。
図9(a)〜(c)に、各サーボモータ31において、修正加速度Aamd、修正速度Vamd、及び修正回転角度θamdを算出する態様を示す。
図9(a)に示すように、前側カウンタCfrtに対応する位置(Cfrt=0)において、サーボモータ31の対応する加速度A[0](破線)に、最大電流比Iratmaxの逆数を掛けて、矢印で示すように修正加速度Aamd[0](実線)を算出する。
そして、図9(b)に示すように、修正加速度Aamd[0]とそれに対応するサーボモータ31の速度V[0]とに基づいて、所定の期間Ts後の修正速度Vamd[1]を算出する。このため、破線で示す速度V[1]は、実線で示す修正速度Vamd[1]に修正される。
さらに、図9(c)に示すように、修正速度Vamd[0]とそれに対応するサーボモータ31の回転角度θ[0]とに基づいて、所定の期間Ts後の修正回転角度θamd[1]を算出する。このため、破線で示す回転角度θ[1]は、実線で示す修正回転角度θamd[1]に修正される。
このようにして、各サーボモータ31において、第1姿勢側から修正加速度Aamd[J][Cfrt]、修正速度Vamd[J][Cfrt]、及び修正回転角度θamd[J][Cfrt]が繰り返し算出される。
図6に戻り、前側カウンタCfrtの値を1増加させる(S21)。すなわち、前側カウンタCfrt=前側カウンタCfrt+1とする。これにより、前側カウンタCfrtに対応する位置は、第1姿勢を開始時点として所定の期間Ts経過後の位置となる。
続いて、ロボット10の第2姿勢(最適化区間の他方の端部)から、各種パラメータの算出が一度も行われていないか否か判定する(S22)。具体的には、後側カウンタCbakの値が最大値Cmaxであるか否か判定する。そして、後側カウンタCbakの値が最大値Cmaxであると判定した場合には移行先1へ進み(S22:YES)、後側カウンタCbakの値が最大値Cmaxでないと判定した場合には移行先2へ進む(S22:NO)。
移行先1へ処理を移行した後、後側カウンタCbakに対応する位置において、S13〜S21の処理に対応するS23〜S31の処理を行う。すなわち、ロボット10の第2姿勢(最適化区間の他方の端部)を開始時点として、後側カウンタCbakに対応して所定の期間Ts毎に各種パラメータを算出する。なお、S13〜S21の処理と同様の処理については説明を簡略化する。
後側カウンタCbakに対応する位置において、ロボット10の各サーボモータ31について、定格の最大電流Imax(上限値)に対する駆動時の電流Iの電流比Iratを算出する(S23)。J番目のサーボモータ31を添え字Jで表し、後側カウンタCbakに対応する位置を添え字Cbakで表すと、電流比Irat[J]=|電流I[J][Cbak]|/最大電流Imax[J]となる。
図10に示すように、最適化区間において、速度連続値(図10(a)参照)、加速度連続値(図10(b)参照)、電流連続値(図10(c)参照)がそれぞれ取得されている。速度連続値では、時間tの最大値からの減少量(右端の第2姿勢を開始時点とすれば時間tの増加量)に比例して速度Vが上昇し、定格の最大速度Vmax(上限値)に達した時点で最大速度Vmaxが維持され、その後に時間の減少量(第2姿勢側からの時間tの増加量)に比例して速度Vが0まで低下している。加速度連続値では、時間tが最大値から減少する(第2姿勢側から時間tが増加する)際に、速度Vが最大速度Vmaxに到達するまで一定の負の加速度に維持され、速度Vが最大速度Vmaxに達した時点で加速度Aが0にされ、その後に速度Vが0に低下するまで一定の正の加速度に維持されている。電流連続値では、所定の負の電流から時間tの減少量(第2姿勢側からの時間tの増加量)に比例して電流Iが減少(負側に増大)し、速度Vが最大速度Vmaxに達した時点で電流Iが増加され、その後に所定の正の電流から時間tの減少量(第2姿勢側からの時間tの増加量)に比例して電流Iが減少している。そして、図10(c)の枠で囲んだ部分を拡大して図10(d)に示すように、後側カウンタCbakに対応する位置において、最大電流Imaxに対する駆動時の電流Iの電流比Iratを算出する。
図7に戻り、各サーボモータ31の電流比Irat[J]のうち、最大値である最大電流比Iratmaxを選択する(S24)。そして、最大電流比Iratmaxの逆数に基づいて、各サーボモータ31の対応する加速度Aを修正した修正加速度Aamdを算出する(S25)。すなわち、後側カウンタCbakに対応する位置において、各サーボモータ31の加速度Aをそれぞれ最大電流比Iratmaxで割って修正加速度Aamdを算出する。詳しくは、J番目のサーボモータ31を添え字Jで表し、後側カウンタCbakに対応する位置を添え字Cbakで表すと、修正加速度Aamd[J][Cbak]=加速度A[J][Cbak]/最大電流比Iratmaxとなる。なお、最大電流比Iratmaxが0である場合には、各サーボモータの加速度A[J][Cbak]を0とする。
続いて、修正加速度Aamdとその修正加速度Aamdに対応する各サーボモータ31の速度に基づいて、各サーボモータ31における第2姿勢側から所定の期間Ts後の速度である修正速度Vamdを算出する(S26)。詳しくは、後側カウンタCbakに対応する位置において、ロボット10の各サーボモータ31について、前回の修正速度Vamd[J][Cbak+1]に前回の修正加速度Aamd[J][cfrt+1]と所定の期間Tsとの積を加えて、修正速度Vamd[J][Cbak]を算出する。すなわち、修正速度Vamd[J][Cbak]=修正速度Vamd[J][Cbak+1]+Aamd[J][cfrt+1]×期間Tsとなる。なお、後側カウンタCbakの値が最大値Cmaxの場合には、修正速度Vamd[J][Cmax]=速度V[J][Cmax]とする。
そして、修正速度Vamdが定格の最大速度Vmaxよりも小さくないと判定した場合には(S27:NO)、修正速度Vamdを最大速度Vmaxとする(S29)。
一方、修正速度Vamdが定格の最大速度Vmaxよりも小さいと判定した場合には(S27:YES)、修正速度Vamdとその修正速度Vamdに対応する各サーボモータ31の回転角度に基づいて、各サーボモータ31における第2姿勢側から所定の期間Ts後の回転角度である修正回転角度θamdを算出する(S28)。詳しくは、後側カウンタCbakに対応する位置において、ロボット10の各サーボモータ31について、前回の修正回転角度θamd[J][Cbak+1]に前回の修正速度Vamd[J][cfrt+1]と所定の期間Tsとの積を加えて、修正回転角度θamd[J][Cbak]を算出する。すなわち、修正回転角度θamd[J][Cbak]=修正回転角度θamd[J][Cbak+1]+Vamd[J][cfrt+1]×期間Tsとなる。なお、後側カウンタCbakの値が最大値Cmaxの場合には、修正回転角度θamd[J][Cmax]=回転角度θ[J][Cmax]とする。
図11(a)〜(c)に、各サーボモータ31において、修正加速度Aamd、修正速度Vamd、及び修正回転角度θamdを算出する態様を示す。
図11(a)に示すように、後側カウンタCbakに対応する位置(Cbak=Cmax)において、サーボモータ31の対応する加速度A[Cmax](破線)に、最大電流比Iratmaxの逆数を掛けて、矢印で示すように修正加速度Aamd[Cmax](実線)を算出する。
そして、図11(b)に示すように、修正加速度Aamd[Cmax]とそれに対応するサーボモータ31の速度V[Cmax]とに基づいて、第2姿勢側から所定の期間Ts後の修正速度Vamd[Cmax−1]を算出する。このため、破線で示す速度V[Cmax−1]は、実線で示す修正速度Vamd[Cmax−1]に修正される。
さらに、図11(c)に示すように、修正速度Vamd[Cmax]とそれに対応するサーボモータ31の回転角度θ[Cmax]とに基づいて、第2姿勢側から所定の期間Ts後の修正回転角度θamd[Cmax−1]を算出する。このため、破線で示す回転角度θ[Cmax−1]は、実線で示す修正回転角度θamd[Cmax−1]に修正される。
このようにして、各サーボモータ31において、第2姿勢側から修正加速度Aamd[J][Cbak]、修正速度Vamd[J][Cbak]、及び修正回転角度θamd[J][Cbak]が繰り返し算出される。
図7に戻り、後側カウンタCbakの値を1減少させる(S31)。すなわち、後側カウンタCbak=後側カウンタCbak−1とする。これにより、後側カウンタCbakに対応する位置は、第2姿勢側から所定の期間Ts経過後の位置となる。
続いて、第1姿勢側から繰り返し算出した修正回転角度θamd[Cfrt]と、第2姿勢側から繰り返し算出した修正回転角度θamd[Cbak]とが重なっているか否か判定する(S32)。具体的には、後側カウンタCbakに対応する位置における修正回転角度θamd[Cbak]が、前側カウンタCfrtに対応する位置における修正回転角度θamd[Cfrt]よりも小さいか否か判定する。
図12に示すように、回転角度θの変動パターンにおいて、第1姿勢側(図の左端側)から修正回転角度θamd[Cfrt]が順次算出されるとともに、第2姿勢側(図の右端側)から修正回転角度θamd[Cbak]が順次算出される。そして、これらの修正回転角度θamd[Cfrt],θamd[Cbak]の変動パターンによりロボット10のサーボモータ31を駆動する場合には、これらの修正回転角度θamd[Cfrt],θamd[Cbak]の間がサーボモータ31を駆動する残り回転角度になる。
図7に戻り、第1姿勢側から算出した修正回転角度θamd[Cfrt]と第2姿勢側から算出した修正回転角度θamd[Cbak]とが重なっていないと判定した場合には(S32:NO)、次回に、第1姿勢側から算出する修正速度Vamd[Cfrt]と、第2姿勢側から算出する修正速度Vamd[Cbak]とのいずれを優先的に算出するか決定する(S33)。具体的には、第1姿勢側から繰り返し算出した修正速度Vamd[Cfrt]が、第2姿勢側から繰り返し算出した修正速度Vamd[Cbak]よりも小さいと判定した場合には(S33:YES)、移行先3へ進んで、次回に第1姿勢側から各種パラメータを算出する(S13〜S21)。一方、第1姿勢側から繰り返し算出した修正速度Vamd[Cfrt]が、第2姿勢側から繰り返し算出した修正速度Vamd[Cbak]よりも小さくないと判定した場合には(S33:NO)、移行先1へ進んで、次回に第2姿勢側から各種パラメータを算出する(S23〜S31)。
図13に示すように、速度Vの変動パターンにおいて、第1姿勢側(図の左端側)から修正速度Vamd[Cfrt]が順次算出されるとともに、第2姿勢側(図の右端側)から修正速度Vamd[Cbak]が順次算出される。このとき、修正速度Vamd[Cfrt]と修正速度Vamd[Cbak]とは必ずしも一致しない。したがって、仮に修正速度Vamd[Cfrt]と修正速度Vamd[Cbak]とを同じ回数だけ繰り返し算出して、修正回転角度θamd[Cfrt],θamd[Cbak]の間の残り回転角度が0になったとすると、その時点で修正速度Vamd[Cfrt]と修正速度Vamd[Cbak]とに差が生じているおそれがある。そこで、上記S33の判定を通じて、修正速度Vamd[Cfrt]と修正速度Vamd[Cbak]とを比較して、低い方の修正速度Vamdについて優先的に次回(所定の期間Ts後)の修正速度Vamdを算出する。
図7に戻り、第1姿勢側から算出した修正回転角度θamd[Cfrt]と第2姿勢側から算出した修正回転角度θamd[Cbak]とが重なっていると判定した場合には(S32:YES)、この一連の処理を終了する(END)。この場合には、第1姿勢を開始時点とした各サーボモータ31の一連の修正回転角度θamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正回転角度θamd[J][Cbak]とが算出されている。また、同様にして、第1姿勢を開始時点とした各サーボモータ31の一連の修正速度Vamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正速度Vamd[J][Cbak]とが算出されている。さらに、同様にして、第1姿勢を開始時点とした各サーボモータ31の一連の修正加速度Aamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正加速度Aamd[J][Cbak]とが算出されている。
なお、S13,S23の処理が電流比算出手段としての処理に相当し、S15,S25の処理が修正加速度算出手段としての処理に相当し、S16,S26の処理が修正速度算出手段としての処理に相当し、S18,S28の処理が修正回転位置算出手段としての処理に相当する。
その後、制御装置20のCPU21は、図14(a),(c)に示すように、第1姿勢を開始時点とした各サーボモータ31の一連の修正速度Vamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正速度Vamd[J][Cbak]とに基づいて、修正後の速度変動パターンを設定する。すなわち、一連の修正速度Vamd[J][Cfrt]と、一連の修正速度Vamd[J][Cbak]とを結合することにより、修正速度変動パターンを設定する。このとき、第2姿勢を開始時点として各サーボモータ31を加速する際の修正速度Vamd[J][Cbak]は、第1姿勢側から各サーボモータ31を第2姿勢で停止する場合に、第2姿勢へ向けて各サーボモータ31を減速する際の速度と等しいと考えることができる。すなわち、第2姿勢から最も高速で各サーボモータ31を駆動する際に各サーボモータ31に供給される電流の大きさは、第1姿勢側から第2姿勢まで最大限減速する際に各サーボモータ31に供給される電流の大きさと等しいと考えることができる。
また、制御装置20のCPU21は、図14(b),(d)に示すように、第1姿勢を開始時点とした各サーボモータ31の一連の修正回転角度θamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正回転角度θamd[J][Cbak]とに基づいて、修正後の回転角度変動パターン(回転位置変動パターン)を設定する。すなわち、一連の修正回転角度θamd[J][Cfrt]と、一連の修正回転角度θamd[J][Cbak]とを結合することにより、修正回転角度変動パターン(修正回転位置変動パターン)を設定する。
同様にして、制御装置20のCPU21は、第1姿勢を開始時点とした各サーボモータ31の一連の修正加速度Aamd[J][Cfrt]と、第2姿勢を開始時点とした各サーボモータ31の一連の修正加速度Aamd[J][Cbak]とに基づいて、修正後の加速度変動パターンを設定する。すなわち、一連の修正加速度Aamd[J][Cfrt]と、一連の修正加速度Aamd[J][Cbak]とを結合することにより、修正加速度変動パターンを設定する。
図15に、最適化前の各種パラメータの変動パターン(図15(a1)〜(d1))と、最適化後の各種パラメータの変動パターン(図15(a2)〜(d2))とを示す。ここでは、ロボット10において、J1番目のサーボモータ及びJ2番目のサーボモータの2つのサーボモータを例にして説明する。
図15(d1)に示すように、J1番目のサーボモータを駆動する電流は、J2番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、J1番目のサーボモータの電流比Irat[J1]が、最大電流比Iratmaxとして選択される。
そして、図15(c1)に示す各サーボモータの加速度A[J]に、最大電流比Iratmaxの逆数が掛けられて、図15(c2)に示す修正加速度Aamd[J]が算出される。ただし、上述したように、第1姿勢側から算出した修正回転角度θamd[J][Cfrt]と第2姿勢側から算出した修正回転角度θamd[J][Cbak]とが重なった時点で、第1姿勢側から算出した各サーボモータ31の一連の修正加速度Aamd[J][Cfrt]と、第2姿勢側から算出した各サーボモータ31の一連の修正加速度Aamd[J][Cbak]とが結合されている。
続いて、図15(b2)に示すように、第1姿勢(図の左端)及び第2姿勢(図の右端)をそれぞれ開始時点として、修正加速度Aamd[J][Cfrt],Aamd[J][Cbak]とその修正加速度に対応する各サーボモータの修正速度Vamd[J][Cfrt],Vamd[J][Cbak]とに基づいて、各サーボモータにおける所定の期間後(次回)の速度である修正速度Vamd[J][Cfrt+1],Vamd[J][Cbak−1]を算出する。このとき、第1姿勢側から算出した修正速度Vamd[J][Cfrt+1]と、第2姿勢側から算出した修正速度Vamd[J][Cbak−1]とを比較して、低い方の修正速度Vamd[J]について優先的に所定の期間後(次回)の修正速度Vamd[J]を算出する。そして、第1姿勢側から算出した修正速度Vamd[J][Cfrt]と第2姿勢側から算出した修正速度Vamd[J][Cbak]とが重なった時点で、第1姿勢側から算出した各サーボモータ31の一連の修正速度Vamd[J][Cfrt]と、第2姿勢側から算出した各サーボモータ31の一連の修正速度Vamd[J][Cbak]とが結合されている。図15(b2)と図15(b1)とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
続いて、図15(a2)に示すように、第1姿勢(図の左端)及び第2姿勢(図の右端)をそれぞれ開始時点として、修正速度Vamd[J][Cfrt],Vamd[J][Cbak]とその修正速度に対応する各サーボモータの修正回転角度θamd[J][Cfrt],θamd[J][Cbak]とに基づいて、各サーボモータにおける所定の期間後(次回)の回転角度である修正回転角度θamd[J][Cfrt+1],θamd[J][Cbak−1]を算出する。この修正回転角度θamd[J]の算出は、修正速度Vamd[J][Cfrt+1]と修正速度Vamd[J][Cbak−1]とのうち、低い方の修正速度Vamd[J]について優先的に所定の期間後(次回)の修正速度Vamd[J]を算出することに併せて行う。そして、第1姿勢側から算出した修正速度Vamd[J][Cfrt]と第2姿勢側から算出した修正速度Vamd[J][Cbak]とが重なった時点で、第1姿勢側から算出した各サーボモータ31の一連の修正回転角度θamd[J][Cfrt]と、第2姿勢側から算出した各サーボモータ31の一連の修正回転角度θamd[J][Cbak]とが結合されている。図15(a2)と図15(a1)とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
図16に、ロボット10が駆動される途中で、電流比Irat[J1]が最大となるサーボモータが替わる場合について説明する。ここでは、J1番目のサーボモータにより回転される回転部に、J2番目のサーボモータにより回転される回転部が接続されている構成を例にして説明する。
J1番目のサーボモータの回転角度が−π/2から+π/2まで変化し、J2番目のサーボモータの回転角度が0から144π/180まで変化する。このとき、図16(a)に示すように、J1番目のサーボモータとJ2番目のサーボモータとが、同時に回転を開始する。そして、図16(b)に示す状態において、J1番目のサーボモータの回転により、J2番目のサーボモータにより回転される回転部に遠心力が作用し、J2番目のサーボモータの負荷が最大となる。このため、J2番目のサーボモータを駆動する電流が、J1番目のサーボモータを駆動する電流よりも大きくなり、両者の大小関係が逆転する。その後、図16(c)に示すように、J1番目のサーボモータとJ2番目のサーボモータとは、それぞれの目標回転角度まで回転する。
図17に、図16で示す場合について、最適化前の各種パラメータの変動パターン(図17(a1)〜(d1))と、最適化後の各種パラメータの変動パターン(図17(a2)〜(d2))とを示す。
図17(d1)に示すように、一点鎖線で囲む範囲のうち左寄りの部分(第1姿勢近傍)では、J1番目のサーボモータを駆動する電流は、J2番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、J1番目のサーボモータの電流比Irat[J1]が、最大電流比Iratmaxとして選択される。そして、一点鎖線で囲む範囲のうち右寄りの部分では、J2番目のサーボモータを駆動する電流は、J1番目のサーボモータを駆動する電流よりも大きく、サーボモータの中で最も大きくなっている。このため、J2番目のサーボモータの電流比Irat[J1]が、最大電流比Iratmaxとして選択される。したがって、一点差線で囲む範囲において、ロボット10が駆動される途中で、電流比Irat[J]が最大となるサーボモータが替わることとなる。
その後、図15に示した場合と同様にして、図17(c1)に示す各サーボモータの加速度A[J]に、最大電流比Iratmaxの逆数が掛けられて、図17(c2)に示す修正加速度Aamd[J]が算出される。そして、図17(b2)に示すように、第1姿勢(図の左端)及び第2姿勢(図の右端)をそれぞれ開始時点として、修正加速度Aamd[J][Cfrt],Aamd[J][Cbak]とその修正加速度に対応する各サーボモータの修正速度Vamd[J][Cfrt],Vamd[J][Cbak]とに基づいて、各サーボモータにおける所定の期間後(次回)の速度である修正速度Vamd[J][Cfrt+1],Vamd[J][Cbak−1]を算出する。この場合も、図17(b2)と図17(b1)とを比較して分かるように、最適化後の速度変動パターンの長さは、最適化前の速度変動パターンの長さよりも短くなっている。
続いて、図17(a2)に示すように、第1姿勢(図の左端)及び第2姿勢(図の右端)をそれぞれ開始時点として、修正速度Vamd[J][Cfrt],Vamd[J][Cbak]とその修正速度に対応する各サーボモータの修正回転角度θamd[J][Cfrt],θamd[J][Cbak]とに基づいて、各サーボモータにおける所定の期間後(次回)の回転角度である修正回転角度θamd[J][Cfrt+1],θamd[J][Cbak−1]を算出する。この場合も、図17(a2)と図17(a1)とを比較して分かるように、最適化後の回転角度変動パターンの長さは、最適化前の回転角度変動パターンの長さよりも短くなっている。
以上詳述した本実施形態は以下の利点を有する。
・各サーボモータ31の加速度A[J]は各サーボモータ31に供給される電流Iに比例することとなり、電流値の倍率に基づいて修正後の加速度を算出することができる。この点、所定の期間毎に、算出された各サーボモータ31の電流比Irat[J]のうち、最も大きい電流比Irat[J]である最大電流比Iratmax[J]が選択され、この最大電流比Iratmax[J]の逆数に基づいて各サーボモータ31の対応する加速度を修正した修正加速度Aamd[J]が算出される。ここで、最大電流比Iratmax[J]の逆数は、電流比Irat[J]の逆数のうちで最も小さな値である。このため、各サーボモータ31の加速度A[J]を同じ比率で大きくする際に、各サーボモータ31において電流値が最大電流Imaxを超えることを抑制しつつ、各サーボモータ31の修正加速度Aamd[J]を最大限大きくすることができる。
・第1姿勢及び第2姿勢をそれぞれ開始時点として、修正速度Vamd[J]を算出しているため、第2姿勢に到達した時点で修正速度Vamd[J]を0にすることができる。このとき、第2姿勢を開始時点として加速する際の修正速度Vamd[J]は、第1姿勢側から第2姿勢で停止する場合に、第2姿勢へ向けて減速する際の速度と等しいと考えることができる。すなわち、第2姿勢から最も高速で駆動する際に各サーボモータ31に供給される電流の大きさは、第1姿勢側から第2姿勢まで最大限減速する際に各サーボモータ31に供給される電流の大きさと等しいと考えることができる。したがって、第1姿勢及び第2姿勢をそれぞれ開始時点として、修正速度Vamd[J]を算出することにより、電流値が最大電流Imaxを超えない範囲で適切に修正速度Vamd[J]を算出することができる。
・第1姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Vamd[J]と、第2姿勢を開始時点として回転角度が重なるまでに繰り返し算出された修正速度Vamd[J]とに基づいて、修正後の速度変動パターンを適切に設定することができる。その結果、ロボット10のアームを第1姿勢と第2姿勢との間で駆動することにより、各サーボモータ31における速度V[J]、加速度A[J]、及び供給される電流Iの各連続値以外のパラメータを必要とすることなく、ロボット10をより高速で動作させるための速度変動パターンを取得することができる。
・また、第1姿勢を開始時点として繰り返し算出された修正回転角度θamd[J]と、第2姿勢を開始時点として繰り返し算出された修正回転角度θamd[J]とが重なる場合に、第1姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Aamd[J]と、第2姿勢を開始時点としてそれまでに繰り返し算出された修正加速度Aamd[J]とに基づいて、修正加速度変動パターンを設定している。したがって、各サーボモータ31における速度V[J]、加速度A[J]、及び供給される電流Iの各連続値以外のパラメータを必要とすることなく、ロボット10をより高速で動作させるための加速度変動パターンを取得することができる。
・第1姿勢を開始時点として繰り返し算出された修正回転角度θamd[J]と、第2姿勢を開始時点として繰り返し算出された修正回転角度θamd[J]とが重なる場合に、第1姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θamd[J]と、第2姿勢を開始時点としてそれまでに繰り返し算出された修正回転角度θamd[J]とに基づいて、修正後の回転角度変動パターンを設定している。したがって、各サーボモータ31における速度V[J]、加速度A[J]、及び供給される電流Iの各連続値以外のパラメータを必要とすることなく、ロボット10をより高速で動作させるための回転角度変動パターンを取得することができる。
・第1姿勢を開始時点とした修正速度Vamd[J]と第2姿勢を開始時点とした修正速度Vamd[J]とにおいて、低い方の修正速度Vamd[J]について優先的に所定の期間後の修正速度Vamd[J]が算出されるため、それらの修正速度Vamd[J]の差を縮めることができる。したがって、第1姿勢を開始時点として算出された修正回転角度θamd[J]と、第2姿勢を開始時点として算出された修正回転角度θamd[J]とが重なる時点で、第1姿勢を開始時点として算出された修正速度Vamd[J]と、第2姿勢を開始時点として算出された修正速度Vamd[J]との差を小さくすることができる。その結果、修正後の速度変動パターンに段差が生じることを抑制することができる。
さらに、第1姿勢を開始時点とした修正速度Vamd[J]と第2姿勢を開始時点とした修正速度Vamd[J]との差が小さい状態を維持しつつ、第1姿勢を開始時点として算出された修正回転角度θamd[J]と、第2姿勢を開始時点として算出された修正回転角度θamd[J]とが重なる時点まで修正速度Vamd[J]が算出される。このため、必要な部分についてのみ修正速度Vamd[J]が算出されるようになり、計算負荷が軽減されることで処理を高速化することができる。
上記実施形態に限定されず、例えば次のように実施することもできる。
・上記実施形態では、最大電流比Iratmaxの逆数を各サーボモータ31の対応する加速度A[J]に掛けて、修正加速度Aamd[J]を算出した。しかしながら、電流比Irat[J]の逆数を各サーボモータ31の対応する加速度A[J]に掛けることにより、そのサーボモータ31では、その修正加速度Aamd[J]を実現する際に電流I[J]がその上限値に一致することとなる。ここで、ロボット10のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、電流値がその定格の最大電流Imax(上限値)に一致するように修正後の各変動パターンを設定すると、アームを実際に駆動する際に電流値が最大電流Imaxを超えるおそれがある。
この点、最大電流比Iratmaxの逆数を各サーボモータ31の対応する加速度A[J]に掛けた値よりも所定度合小さくなるように、修正加速度Aamd[J]を算出するといった構成を採用することが有効である。具体的には、最大電流比Iratmaxの逆数を各サーボモータ31の対応する加速度A[J]に掛けたものに、所定の係数k(例えばk=0.9)を掛けて修正加速度Aamd[J]を算出するとよい。こうした構成によれば、最大電流Imaxに相当する修正加速度Aamd[J]よりも所定度合小さい修正加速度Aamd[J]を算出することができ、修正後の変動パターンによりアームを実際に駆動する際に電流値が最大電流Imaxを超えることを抑制することができる。
・上記のように、ロボット10のアームの動作は、摩擦や慣性等の多くのパラメータの影響を受ける。このため、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンが設定された場合であっても、実際のアームの駆動に対してそれらの変動パターンが最適であるとは限らず、変動パターンをより最適化する余地がある。
この点、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも1つが設定された後に、その設定された修正後の変動パターンとなるように、再度、アームを第1姿勢と第2姿勢との間で駆動するとともに、その駆動中に各サーボモータ31において速度V[J]、加速度A[J]、及び供給される電流I[J]の各連続値を検出するといった構成を採用することが有効である。
上記構成によれば、修正後の速度変動パターン、修正後の加速度変動パターン、及び修正後の回転角度変動パターンの少なくとも1つが設定された後に、再度、各サーボモータ31において速度V[J]、加速度A[J]、及び供給される電流I[J]の各連続値が検出される。このため、その各連続値に基づいて、再度、電流比Irat[J]、修正加速度Aamd[J]、修正速度Vamd[J]、及び修正回転角度θamd[J]がそれぞれ算出される。したがって、一度設定された修正後の変動パターンに基づいて、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
さらに、上記構成と、最大電流比Iratmaxの逆数を各サーボモータ31の対応する加速度A[J]に掛けた値よりも所定度合小さくなるように、修正加速度Aamd[J]を算出する構成とを組み合わせることにより、修正後の変動パターンによりアームを実際に駆動する際に、電流値が最大電流Imaxを超えることを抑制しつつ、修正後の速度変動パターンや、修正後の加速度変動パターン、修正後の回転角度変動パターンを、より最適化することができる。
・上記実施形態では、電流比Irat[J]=|電流I[J][Cfrt]|/最大電流Imax[J]としたが、電流比Irat[J]=最大電流Imax[J]/|電流I[J][Cfrt]|とし、それに整合させるように図6のS14,S15の計算式を変更してもよい。
・上記実施形態では、第1姿勢側から算出された修正速度Vamd[J][Cfrt]と、第2姿勢側から算出された修正速度Vamd[J][Cbak]とを比較して、低い方の修正速度Vamd[J]について優先的に次回の(所定の期間Ts後)の修正速度Vamd[J]を算出するようにした。しかしながら、Vamd[J]を算出する手順はこれに限定されず、修正速度Vamd[J][Cfrt]と修正速度Vamd[J][Cbak]とを同一の所定回数算出して、両者の差が所定値よりも大きくなった場合に、低い方の修正速度Vamd[J]について優先的に次回の修正速度Vamd[J]を算出してもよい。また、第1姿勢側から繰り返し算出した修正回転角度θamd[J][Cfrt]と、第2姿勢側から繰り返し算出した修正回転角度θamd[J][Cbak]とが重なるまで、修正速度Vamd[J][Cfrt]と修正速度Vamd[J][Cbak]とを同一回数算出してもよい。
これらの場合であっても、第1姿勢及び第2姿勢をそれぞれ開始時点として、修正速度Vamd[J][Cfrt],Vamd[J][Cbak]が算出されるため、第1姿勢及び第2姿勢から修正加速度Aamd[J][Cfrt],Aamd[J][Cbak]に基づいて徐々に修正速度Vamd[J][Cfrt],Vamd[J][Cbak]を大きくすることができる。さらに、第1姿勢及び第2姿勢をそれぞれ開始時点として、修正速度Vamd[J]を算出しているため、第2姿勢に到達した時点で修正速度Vamd[J]を0にすることができる。これに対して、例えば第1姿勢を開始時点として第2姿勢まで順に修正速度Vamd[J][Cfrt]を算出した場合には、修正回転角度θamd[J][Cfrt]が第2姿勢まで到達した時に、修正速度Vamd[J][Cfrt]は0から離れた値となる。このため、第2姿勢において修正速度Vamd[J][Cfrt]を0にするためには、各サーボモータ31に大きな電流の供給を要求する必要があり、電流値が最大電流Imaxを超えることとなる。
・上記実施形態では、修正速度変動パターン、修正加速度変動パターン、及び修正回転角度変動パターンを設定したが、これらの少なくとも1つを設定するようにしてもよい。
・上記実施形態では、各サーボモータ31の電流比Irat[J]についても、修正速度Vamd[J][Cfrt]と修正速度Vamd[J][Cbak]とのうち、優先的に次回算出される修正速度Vamd[J]に併せて、対応する電流比Irat[J]を算出した。しかしながら、各サーボモータ31の電流比Irat[J]を、第1姿勢側から又は第2姿勢側から予め全て算出しておき、修正加速度Aamd[J]を算出する際に対応する電流比Irat[J]を読み込んで使用してもよい。
・垂直多関節型の6軸ロボットに限らず、水平多関節型の4軸ロボット等、それ以外の型式や軸数のロボットを採用することもできる。