(57)【特許請求の範囲】
【請求項１】
多関節ロボットの各関節が、前記各関節の教示位置をベクトル成分として含む複数の教示点を順次辿って動作するよう、前記各関節を駆動する各アクチュエータの動作を制御する駆動制御部に、所定時間間隔で位置指令を出力するロボット制御装置において、
前記各教示点の間を補間する複数の補間教示点を通過する補間関数を求める補間関数計算処理と、
前記各補間教示点に含まれる各ベクトル成分を前記補間関数の変数で微分した微分係数をそれぞれ計算する微分係数計算処理と、
前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理と、
前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理と、
前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記各通過速度の逆数を前記変数で積分して得られる目的関数を最小化する最短時間制御処理と、
前記最短時間制御処理で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理と、
前記通過速度関数を前記所定時間間隔の値で時間積分する度に、該積分の値を前記補間関数における前記変数に代入することで、前記位置指令を生成する位置指令生成処理と、を実行する演算部を備えたことを特徴とするロボット制御装置。
【請求項２】
前記演算部は、前記微分係数計算処理として、前記各補間教示点の各ベクトル成分に対して前記変数を前記各補間教示点の間隔よりも小さい微小値で変化させたときの各ベクトル成分を前記補間関数に基づいて計算し、該計算の結果を用いて、前記各微分係数を計算することを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記演算部は、前記補間関数計算処理として、前記各教示点に、時刻に対応した教示点番号を付与し、該教示点番号を用いたスプライン補間により前記補間関数を求めることを特徴とする請求項１又は２に記載のロボット制御装置。
【請求項４】
前記許容速度が、前記変数の値に応じて変化する値に設定されていることを特徴とする請求項１乃至３のいずれか１項に記載のロボット制御装置。
【請求項５】
前記許容速度が、前記変数の値が増加するに連れて小さくなるように設定されていることを特徴とする請求項４に記載のロボット制御装置。
【請求項６】
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節について前記推定速度を時間で１階微分した推定加速度を、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定加速度と予め定めた許容加速度との比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至５のいずれか１項に記載のロボット制御装置。
【請求項７】
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節について前記推定速度を時間で２階微分した推定加加速度を、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定加加速度と予め定めた許容加加速度との比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至６のいずれか１項に記載のロボット制御装置。
【請求項８】
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節の推定トルクを、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定トルクと予め定めた許容トルクとの比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至７のいずれか１項に記載のロボット制御装置。
【請求項９】
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記多関節ロボットの先端の推定先端速度を、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定先端速度と予め定めた許容先端速度との比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至８のいずれか１項に記載のロボット制御装置。
【請求項１０】
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記多関節ロボットの先端の推定回転速度を、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定回転速度と予め定めた許容回転速度との比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至９のいずれか１項に記載のロボット制御装置。
【請求項１１】
前記アクチュエータが、電動モータ及び減速機を有しており、
前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記電動モータの推定モータトルクを、前記減速機の減速比、前記各通過速度及び前記各微分係数に基づいて計算し、
前記制約条件には、更に、前記推定モータトルクと予め定めた許容モータトルクとの比が前記所定制約範囲内となる条件が含まれる、
ことを特徴とする請求項１乃至１０のいずれか１項に記載のロボット制御装置。
【請求項１２】
多関節ロボットと、
前記多関節ロボットを制御する請求項１乃至１１のいずれか１項に記載のロボット制御装置と、を備えたことを特徴とするロボット装置。
【請求項１３】
多関節ロボットの各関節が、前記各関節の教示位置をベクトル成分として含む複数の教示点を順次辿って動作するよう、前記各関節を駆動する各アクチュエータの動作を制御する駆動制御部に、所定時間間隔で出力する位置指令を生成する演算部を有するロボット制御装置を用いたロボット制御方法において、
前記演算部が、前記各教示点の間を補間する複数の補間教示点を通過する補間関数を求める補間関数計算処理工程と、
前記演算部が、前記各補間教示点に含まれる各ベクトル成分を前記補間関数の変数で微分した微分係数をそれぞれ計算する微分係数計算処理工程と、
前記演算部が、前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理工程と、
前記演算部が、前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理工程と、
前記演算部が、前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記各通過速度の逆数を前記変数で積分して得られる目的関数を最小化する最短時間制御工程と、
前記演算部が、前記最短時間制御工程で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理工程と、
前記演算部が、前記通過速度関数を前記所定時間間隔の値で時間積分する度に、該積分の値を前記補間関数における前記変数に代入することで、前記位置指令を生成する位置指令生成処理工程と、を備えたことを特徴とするロボット制御方法。
【請求項１４】
コンピュータに請求項１３に記載のロボット制御方法の各工程を実行させるためのプログラム。
【請求項１５】
請求項１４に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、教示点で与えられる軌道に沿って多関節ロボットを動作させるとき、動作時間が最短になる位置指令を生成するロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体に関する。
【背景技術】
【０００２】
近年、人間の手のように複雑で高速な組立を実現できる、多関節ロボットを有する組立装置の開発事例が増えており、多関節ロボットの高速動作が重要となっている。
【０００３】
しかも、複雑で精密な組立て対象に対応するためには、２つの教示点間を結ぶ単純なＰＴＰ（Ｐｏｉｎｔ  ｔｏ  Ｐｏｉｎｔ）軌道ではなく、３つ以上の教示点を結ぶ複雑なＣＰ（Ｃｏｎｔｉｎｕｏｕｓ  Ｐａｔｈ）軌道に対応する必要がある。
【０００４】
このような要求に応えるべく、多関節ロボットの最短時間制御（Ｔｉｍｅ  Ｏｐｔｉｍａｌ  Ｃｏｎｔｒｏｌ）の技術が提案されている（特許文献１参照）。この最短時間制御は、１９８０年代から論文が散見され、よく知られている。特許文献１では、多関節ロボットの各関節に対する位置指令（目標位置）を所定時間間隔（例えば１ｍｓ間隔）で出力するために、与えられた各教示点間を所定時間間隔で補間して位置指令を求めている。そして、各位置指令の値を、動作時間が最小化するように修正している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１１−１９８０７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１では、最短時間制御に要する計算量が多い。具体的に説明すると、上記特許文献１では、各教示点に基づき目標位置を示す位置指令を計算し、この計算した位置指令に対して最短時間制御の演算を行い、修正した位置指令を生成している。
【０００７】
位置指令は、多関節ロボットの速度や位置精度が低下するのを抑制するために、微小（例えば数ミリ秒）の所定時間間隔で出力する必要があり、データ量が多い。例えば多関節ロボットを１０秒間動かす際に、１ｍｓ間隔で位置指令を出力する際には、１０×１０００＝１万個の修正した位置指令が必要となる。従って、最短時間制御で演算処理するデータ量が多くならざるを得ない。そして、最短時間制御は、最適化問題に帰着して解くものであり、計算時間を要するものであり、データ量の増加は計算時間の増加となる。演算処理している間は、多関節ロボットを動作させることができないので、最短時間制御の目的であった、多関節ロボットの高速動作ができていないという問題があった。
【０００８】
そこで、本発明は、最短時間制御に要する計算量を低減することを目的とするものである。
【課題を解決するための手段】
【０００９】
本発明は、多関節ロボットの各関節が、前記各関節の教示位置をベクトル成分として含む複数の教示点を順次辿って動作するよう、前記各関節を駆動する各アクチュエータの動作を制御する駆動制御部に、所定時間間隔で位置指令を出力するロボット制御装置において、前記各教示点の間を補間する複数の補間教示点を通過する補間関数を求める補間関数計算処理と、前記各補間教示点に含まれる各ベクトル成分を前記補間関数の変数で微分した微分係数をそれぞれ計算する微分係数計算処理と、前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理と、前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理と、前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記各通過速度の逆数を前記変数で積分して得られる目的関数を最小化する最短時間制御処理と、前記最短時間制御処理で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理と、前記通過速度関数を前記所定時間間隔の値で時間積分する度に、該積分の値を前記補間関数における前記変数に代入することで、前記位置指令を生成する位置指令生成処理と、を実行する演算部を備えたことを特徴とする。
【００１０】
また、本発明は、多関節ロボットの各関節が、前記各関節の教示位置をベクトル成分として含む複数の教示点を順次辿って動作するよう、前記各関節を駆動する各アクチュエータの動作を制御する駆動制御部に、所定時間間隔で出力する位置指令を生成する演算部を有するロボット制御装置を用いたロボット制御方法において、前記演算部が、前記各教示点の間を補間する複数の補間教示点を通過する補間関数を求める補間関数計算処理工程と、前記演算部が、前記各補間教示点に含まれる各ベクトル成分を前記補間関数の変数で微分した微分係数をそれぞれ計算する微分係数計算処理工程と、前記演算部が、前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理工程と、前記演算部が、前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理工程と、前記演算部が、前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記各通過速度の逆数を前記変数で積分して得られる目的関数を最小化する最短時間制御工程と、前記演算部が、前記最短時間制御工程で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理工程と、前記演算部が、前記通過速度関数を前記所定時間間隔の値で時間積分する度に、該積分の値を前記補間関数における前記変数に代入することで、前記位置指令を生成する位置指令生成処理工程と、を備えたことを特徴とする。
【発明の効果】
【００１１】
本発明によれば、各補間教示点における補間関数の変数に対する各通過速度を目的関数が最小化するように変更する最短時間制御処理を行い、このようにして求められた各通過速度を用いて、補間関数から所定時間間隔毎の位置指令を生成している。このように、位置指令は目的関数の最小化を行った後に求めているので、補間教示点の数を位置指令の数と同数にする必要はなく、補間教示点の数を、位置指令の数よりも少なくすることが可能である。したがって、最短時間制御処理に要する計算量を低減することができ、演算部において位置指令を求める計算速度を向上させることができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。
【図２】ロボット制御装置の構成を示すブロック図である。
【図３】ロボット制御装置の機能ブロック図である。
【図４】（ａ）は教示点を説明するための模式図である。（ｂ）は補間教示点及び補間関数を説明するための模式図である。（ｃ）は通過速度を説明するための模式図である。
【図５】位置指令を説明するための模式図である。
【図６】本発明の第２実施形態に係るロボット制御装置の微分係数計算処理部の動作を説明するための模式図である。
【図７】（ａ）は本発明の第３実施形態に係るロボット装置の概略構成を示す説明図である。（ｂ）は各ロボットアームに対する教示点列を示す模式図である。
【図８】本発明の第４実施形態に係るロボット制御装置における制約条件違反率計算処理部の処理動作を説明するための図である。
【図９】（ａ）は本発明の第４実施形態に係る電動モータのモデルを示す図、（ｂ）は許容モータトルクを説明するための図である。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１４】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。図１に示すロボット装置１００は、多関節ロボットとしてのロボットアーム２００と、ロボットアーム２００を制御するロボット制御装置３００と、を備えている。また、ロボット装置１００は、ロボット制御装置３００に複数の教示点のデータを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、人が操作するものであり、ロボットアーム２００やロボット制御装置３００の動作を指定するのに用いる。
【００１５】
ロボットアーム２００は、本第１実施形態では、例えば６関節ロボットである。ロボットアーム２００は、各関節Ｊ１〜Ｊ６を各関節軸Ａ１〜Ａ６まわりにそれぞれ回転駆動する複数（６つ）のアクチュエータ２０１〜２０６を有している。ロボットアーム２００は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に手先（ロボットアームの先端）を向けることができる。一般に、ロボットアーム２００の位置と姿勢は、座標系で表現することができる。図１中のＴｏはロボットアーム２００の台座に固定した座標系を表し、Ｔｅはロボットアーム２００の手先に固定した座標系を表す。
【００１６】
本実施形態では、各アクチュエータ２０１〜２０６は、電動モータ２１１〜２１６と電動モータ２１１〜２１６に接続された減速機２２１〜２２６とからなる。なお、各アクチュエータ２０１〜２０６の構成は、これに限定するものではなく、例えば人工筋肉等であってもよい。
【００１７】
本第１実施形態では、各関節Ｊ１〜Ｊ６が回転関節である場合について説明する。この場合、「関節の位置」とは、関節の角度を意味する。なお、各関節が直動関節の場合であってよい。その場合、「関節の位置」とは、直動関節の位置である。同様に、それらの時間微分についても「関節の速度」、「関節の加速度」、「関節の加加速度」という。
【００１８】
ロボットアーム２００は、更に、各アクチュエータ２０１〜２０６の電動モータ２１１〜２１６を駆動制御する駆動制御部としてのサーボ制御装置２３０を有している。サーボ制御装置２３０は、入力した位置指令（目標位置）に基づき、各関節Ｊ１〜Ｊ６の位置が位置指令に追従するよう、各電動モータ２１１〜２１６に電流指令を出力し、各電動モータ２１１〜２１６の動作を制御する。
【００１９】
ロボット制御装置３００は、ティーチングペンダント４００から複数の教示点（教示点列）の入力を受ける。ロボット制御装置３００は、ロボットアーム２００の各関節Ｊ１〜Ｊ６が、複数の教示点を順次辿って動作するよう、教示点に基づき、サーボ制御装置２３０に所定時間間隔で出力する位置指令を生成し、所定時間間隔で位置指令を出力する。
【００２０】
教示点は、各関節（６つの関節）Ｊ１〜Ｊ６の教示位置をベクトル成分として含むベクトル（教示点ベクトル）である。そして、ロボット制御装置３００において最終的に求められる位置指令は、各関節（６つの関節）Ｊ１〜Ｊ６の目標位置をベクトル成分として含むベクトルである。本第１実施形態では、教示点と位置指令とは、点数は異なるが、次元（単位）は同一である。
【００２１】
つまり、ロボット制御装置３００は、入力した教示点からＣＰ軌道の計算をして軌道に沿った多数の位置指令を生成し、これら位置指令を所定時間間隔でサーボ制御装置２３０に出力するものである。
【００２２】
その際、ロボット制御装置３００は、与えられた教示点列によって定まる軌道に対し、軌道上の通過速度を、ロボットアーム２００の各関節Ｊ１〜Ｊ６の速度、加速度、トルク等の制約条件を越えない範囲で調整する、多関節ロボットの最短時間制御を行う。
【００２３】
ここで、教示によるロボットアーム２００の動作方法として次の２種類が考えられる。
【００２４】
１  関節空間移動  Ｍｏｔｉｏｎ  ｉｎ  ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅ
ロボットアーム２００の関節角度で教示点を指定し、指定した教示点で決定される軌道に沿ってロボットを動かす方法。
【００２５】
２  タスク空間移動  Ｍｏｔｉｏｎ  ｉｎ  ｔａｓｋ  ｓｐａｃｅ
三次元的な位置と姿勢で教示点を指定し、指定した教示点とロボットアーム２００の手先で決定される軌道に沿ってロボットアーム２００を動かす方法。
【００２６】
図１では、４つの教示点ｐ１，ｐ２，ｐ３，ｐ４を模式的に図示している。いずれの場合でもロボットアーム２００の動作は教示点で決まる軌道に沿って動く。ここで、教示点はいわゆる３次元空間内の点ではない。関節空間移動の場合では、教示点は全関節Ｊ１〜Ｊ６の関節角度（全モータの回転角度）である。タスク空間移動の場合では、教示点は３次元空間の位置および姿勢であり、これは図１のように、ｐ１，ｐ２，ｐ３，ｐ４を座標系として表現される。
【００２７】
以下、本第１実施形態では、関節空間移動の場合について説明する。関節空間移動では、教示点はロボットの各関節Ｊ１〜Ｊ６の角度である。従って、６軸６関節ロボットの場合、１つの教示点には６つのベクトル成分が含まれる。
【００２８】
図２は、ロボット制御装置３００の構成を示すブロック図である。本第１実施形態では、ロボット制御装置３００は、図２に示すように、演算部としてのＣＰＵ３０１を備えたコンピュータである。
【００２９】
ロボット制御装置３００は、演算部としてのＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及び各種のインタフェース３０６〜３０９を備えている。
【００３０】
ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及び各種のインタフェース３０６〜３０９が、バス３１０を介して接続されている。ＲＯＭ３０２には、ＣＰＵ３０１を動作させるためのプログラム３３０が格納されている。このプログラム３３０は、ＣＰＵ３０１に各種演算処理を実行させるためのプログラムである。ＣＰＵ３０１は、ＲＯＭ３０２に格納されたプログラム３３０に基づいて各種演算処理を実行する。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果を一時的に記憶する記憶部である。ＨＤＤ３０４は、演算処理結果である各種のデータを記憶するための記憶部である。
【００３１】
ティーチングペンダント４００はインタフェース３０６に接続されており、ＣＰＵ３０１はインタフェース３０６及びバス３１０を介してティーチングペンダント４００からの教示点のデータの入力を受ける。
【００３２】
ＨＤＤ３０４には、ＣＰＵ３０１による演算結果を示すデータ（位置指令のデータ）等がＣＰＵ３０１の指令の下で記憶される。
【００３３】
ロボットアーム２００のサーボ制御装置２３０は、インタフェース３０９に接続されており、ＣＰＵ３０１は、位置指令のデータを所定時間間隔でバス３１０及びインタフェース３０９を介してサーボ制御装置２３０に出力する。
【００３４】
インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３２２が接続可能に構成されている。記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。
【００３５】
図３は、ロボット制御装置３００の機能ブロック図である。ＣＰＵ３０１は、ＲＯＭ３０２に格納されたプログラム３３０を実行することで、以下に説明する各処理を実行する各種の処理部３５１，３５２，３５３として機能する。つまり、ＣＰＵ３０１は、プログラム３３０に基づき、図３に示す第１補間部３５１、通過速度計算部３５２、第２補間部３５３として機能する。なお、図３において、丸四角はデータを、四角はＣＰＵ３０１の処理を表す。
【００３６】
各部３５１，３５２，３５３の動作について大まかに説明すると、まず、第１補間部３５１は、補間関数を用いて各教示点の間を補間し、補間教示点のそれぞれの位置において、各関節の位置を求める。更に、第１補間部３５１は、その１階微分値、２階微分値、３階微分値を計算する。
【００３７】
通過速度計算部３５２は、補間教示点のそれぞれの位置において通過速度を定義する。次いで、通過速度計算部３５２は、通過速度の１階微分値と、通過速度の２階微分値を計算する。通過速度計算部３５２は、それらと、第１補間部３５１で計算した各関節の位置、およびその１階微分値、２階微分値、３階微分値とから、ロボットアーム２００の各関節の、推定速度、推定加速度、推定加加速度、推定トルクを計算する。更に、通過速度計算部３５２は、ロボットアーム２００の先端の推定先端速度（以下、推定手先速度という）、およびロボットアーム２００の先端の推定回転速度（以下、推定手先回転速度という）を計算する。
【００３８】
次いで、通過速度計算部３５２は、計算した推定速度、推定加速度、推定加加速度、推定トルク、推定手先速度および推定手先回転速度と、予め定めておいた制約値との比である制約条件違反率を計算する。
【００３９】
通過速度計算部３５２は、計算した全て、または予め選択した制約条件違反率が、所定制約範囲内（±１の範囲内）となる不等式制約を満足し、通過速度の逆数の和、すなわち動作時間を最短にする、不等式制約最適化問題を解いて、通過速度を求める。
【００４０】
第２補間部３５３は、計算した通過速度を所定時間間隔で積分して求めた位置における、ロボットアーム２００の各関節への位置指令を、補間関数を使って計算する。
【００４１】
ここで、数式における記号を以下のように定義する。なお、数式中、太文字はベクトルを示す。
【００４２】
【数１】
0005896789.tif 000002
【００４３】
以下、各部３５１，３５２，３５３の動作について、図３、図４及び図５を参照しながら詳細に説明する。第１補間部３５１は、図３に示すように、補間関数計算処理部３６１と、微分係数計算処理部３６２とを有している。通過速度計算部３５２は、仮設定処理部３６３と、通過速度微分係数計算処理部３６４と、推定値計算処理部３６５と、制約条件違反率計算処理部３６８と、最短時間制御処理部３６９と、通過速度関数計算処理部３７２と、を有している。第２補間部３５３は、位置指令生成処理部３７３からなる。推定値計算処理部３６５は、計算処理部３６６と、運動学計算処理部３６７とからなる。最短時間制御処理部３６９は、収束判定処理部３７０と、通過速度変更処理部３７１とからなる。
【００４４】
まず、補間関数計算処理部３６１は、各教示点（ベクトル）ｐｉの間を補間する複数の補間教示点（ベクトル）ｑ（ｓ）を通過する補間関数を求める補間関数計算処理を実行する（補間関数計算処理工程）。例えば、補間関数計算処理部３６１は、図４（ａ）に示す４つの教示点ｐ１〜ｐ４を取得した場合、４つの教示点ｐ１〜ｐ４の間を補間する、図４（ｂ）に示す複数の補間教示点ｑ（１），ｑ（２），…を通過する補間関数を求める。教示点ｐ１は始点であり、教示点ｐ４は終点である。
【００４５】
ここで、変数ｓは、各補間教示点において連番となる変数、つまり、各補間教示点において、各補間教示点に通過順に割り振った通し番号となる変数であり、補間関数において、実数の値を取り得る。
【００４６】
図４（ｂ）では、補間関数を点線で模式的に表している。補間関数は直線補間や円弧補間、スプライン補間など様々なものが提案されているが、全て、次の式（１）で表現できる。
【００４７】
【数２】
0005896789.tif 000003
【００４８】
ここで、変数ｓは、整数の値とした場合、各補間教示点ｑ（ｓ）の番号となる。例えば、各教示点ｐ１〜ｐ４間を１０分割した場合、３１個の分割した補間教示点ｑ（１）〜ｑ（３１）が得られる。また、補間するので変数ｓは整数でなく連続した数（実数）の値を取り得る。
【００４９】
補間関数は変数ｓの多項式とするのが好ましく、本第１実施形態では、後に３階の微分係数まで計算するので、３次多項式として、次のような式としている。
【００５０】
【数３】
0005896789.tif 000004
【００５１】
式（２）に示す多項式の各係数（ベクトル）ａ１，ａ２，ａ３，ａ４は、ｐｉ−ｐｉ−１区間ごとに異なるものを選択し、本実施形態では、区間の境界で２次の微分係数まで連続なものが選ばれる。
【００５２】
この補間関数には、各教示点ｐｉ上を通過するものと通過しないものとがある。つまり、補間関数は、必ずしも全ての教示点上を通過しなければならないというものではない。
【００５３】
このように、補間関数計算処理部３６１は、式（２）に示す各係数ａ１，ａ２，ａ３，ａ４を計算して、補間関数を求め、各補間教示点ｑ（１），ｑ（２），…を、補間関数の変数ｓにｓ＝１，２，…を代入して求める。
【００５４】
ここで、補間教示点ｑ（１），ｑ（２），…は、後述する通過速度を求めるために用いられるものであるので、その点数を、位置指令の数ほど多くする必要はない。例えば教示点ｐｉ−ｐｉ−１の間を１０分割すれば十分である。補間教示点ｑ（ｓ）の点数は、サーボ制御装置２３０に出力する位置指令の点数よりも少なく設定される。例えば、ロボットアーム２００が図４（ａ）に示した４つの教示点ｐ１〜ｐ４を１０秒間かけて移動するものとし、所定時間間隔１ｍｓでサーボ制御装置２３０に位置指令を出力する場合、１０÷０．００１＝１００００点の位置指令が必要である。これに対し、教示点ｐ１〜ｐ４間を１０分割したとすると、補間教示点ｑ（ｓ）の点数は３１点となり、位置指令の点数に対して格段に少なくできる。
【００５５】
次に、微分係数計算処理部３６２は、各補間教示点ｑ（１），ｑ（２），…に含まれる教示位置を示す各ベクトル成分を補間関数の変数ｓで微分した微分係数をそれぞれ計算する微分係数計算処理を実行する（微分係数計算処理工程）。
【００５６】
ここで、仮に補間教示点ｑ（１）〜ｑ（３１）の差分を計算する方法では、補間誤差が拡大され、高階微分には使えない。
【００５７】
本第１実施形態では、３階微分まで使うので、従来技術のような単純な差分式は使わず、直接、補間関数を微分した関数を用いる。つまり、式（２）をそれぞれ、変数ｓで１階微分、２階微分、３階微分すると、次の式（３）のように計算される。
【００５８】
【数４】
0005896789.tif 000005
【００５９】
したがって、本第１実施形態では、微分係数計算処理部３６２は、各補間教示点ｑ（１）〜ｑ（３１）における各ベクトル成分を変数ｓで微分した微分係数として、１階の微分係数、２階の微分係数、３階の微分係数を、式（３）を用いて計算する。つまり、微分係数計算処理部３６２は、各補間教示点での微分係数を、式（３）の変数ｓにｓ＝１，２，…，３１をそれぞれ代入して計算する。
【００６０】
次に、仮設定処理部３６３は、各補間教示点ｑ（１）〜ｑ（３１）において変数ｓを時間微分した微分値（１階の微分係数）を示す通過速度をそれぞれ仮設定する仮設定処理を実行する（仮設定処理工程）。各通過速度は、各補間教示点ｑ（１）〜ｑ（３１）において変数ｓが変化する速度を示している。
【００６１】
例えば、各通過速度は、図４（ｃ）に示すように、各補間教示点においてφ（１）〜φ（３１）が初期設定（仮設定）される。これら通過速度φ（ｓ）は、
【００６２】
【数５】
0005896789.tif 000006
の式で定義される。
【００６３】
例えば各通過速度φ（１）〜φ（３１）は一定の数字、例えば、
【００６４】
【数６】
0005896789.tif 000007
などに設定される。これは１秒かけて、補間教示点を１つ進むことを意味している。
【００６５】
次に、通過速度微分係数計算処理部３６４は、各補間教示点ｑ（１）〜ｑ（３１）における各通過速度φ（１）〜φ（３１）を変数ｓで微分した微分係数（本実施形態では、１階微分、２階微分）を計算する計算処理を実行する。
【００６６】
この通過速度微分係数計算処理では、次式（４）で表す数式に基づき、計算処理を実行する。
【００６７】
【数７】
0005896789.tif 000008
【００６８】
次に、計算処理部３６６は、ロボットアーム２００の各関節Ｊ１〜Ｊ６の推定速度、推定加速度、推定加加速度を、各通過速度及び各微分係数に基づいて計算する推定値計算処理を実行する（推定値計算処理工程）。つまり、計算処理部３６６は、推定速度、推定加速度、推定加加速度を、各通過速度φ（ｓ）の値（これらに基づいて計算した微分係数φ’（ｓ）、φ’’（ｓ）も含む）、及び各微分係数ｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）の値に基づいて計算する。ここで、ｓ＝１，２，…，３１である。
【００６９】
具体的に説明すると、計算処理部３６６は、以下の式（５）に各計算結果を代入して、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）、推定加加速度ｑ（…）（ｓ）を計算する。これら推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）、推定加加速度ｑ（…）（ｓ）はベクトルである。
【００７０】
【数８】
0005896789.tif 000009
【００７１】
なお、推定加速度とは、推定速度を時間で１階微分した値であり、推定加加速度とは、推定速度を時間で２階微分した値である。
【００７２】
次に、運動学計算処理部３６７は、各補間教示点における各関節の推定トルク、ロボットアーム２００の推定手先速度、及びロボットアーム２００の推定手先回転速度を、各通過速度及び各微分係数に基づいて計算する推定値計算処理を実行する。
【００７３】
具体的に説明すると、運動学計算処理部３６７は、補間教示点（関節の教示位置）ｑ（ｓ）、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）から、関節にかかる推定トルク（ベクトル）ｕ（ｓ）を計算する。
【００７４】
この計算はロボットアーム２００の逆動力学計算として知られている。例えば文献、高野晴久、ロボットの運動学、２００５年６月１０日、オーム社（東京）、９１ページから引用すると、次式となる。
【００７５】
【数９】
0005896789.tif 000010
【００７６】
右辺第１項は慣性項、第２項はコリオリ項、第３項は重力項、第４項は外力項と呼ばれている。ロボットアーム２００を構成するリンクの配置や質量、慣性テンソルおよび各関節の位置ｑ（ｓ）、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）から計算する。
【００７７】
また、運動学計算処理部３６７は、ロボットアーム２００の各関節の補間教示点ｑ（ｓ）、推定速度ｑ（・）（ｓ）から、ロボットアーム２００の推定手先速度（ベクトル）ｒ（・）（ｓ）及び推定手先回転速度（ベクトル）ｗ（・）（ｓ）を計算する。
【００７８】
この計算は順運動学計算として知られており、ヤコビ行列Ｔを使って簡単に求まる。例えば文献、高野晴久、ロボットの運動学、２００５年６月１０日、オーム社（東京）、４２ページから引用すると、次式となる。
【００７９】
【数１０】
0005896789.tif 000011
【００８０】
このように、運動学計算処理部３６７は、関節の位置、速度、加速度、加加速度の推定値と、ロボットの各関節の受け持つ質量や慣性テンソルから、式（６）を用いて、関節にかかる推定トルクを計算する。また、運動学計算処理部３６７は、式（７）を用いて、ロボットアーム２００の推定手先速度及び推定手先回転速度を計算する。
【００８１】
次に、制約条件違反率計算処理部３６８は、各関節の推定速度と予め定めた許容速度との比である関節速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００８２】
【数１１】
0005896789.tif 000012
を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定速度は、式（５）を用いて、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づいて計算される。
【００８３】
また、制約条件違反率計算処理部３６８は、各関節の推定加速度と予め定めた許容加速度との比である関節加速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００８４】
【数１２】
0005896789.tif 000013
を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定加速度は、式（５）を用いて、各通過速度（φ（ｓ）、各通過速度の１階微分値φ’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ））に基づいて計算される。
【００８５】
また、制約条件違反率計算処理部３６８は、各関節の推定加加速度と予め定めた許容加加速度との比である関節加加速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００８６】
【数１３】
0005896789.tif 000014
を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定加加速度は、式（５）を用いて、各通過速度（φ（ｓ）、１階微分値φ’（ｓ）２階微分値φ’’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ）、ｑ’’’（ｓ））に基づいて計算される。
【００８７】
また、制約条件違反率計算処理部３６８は、各関節の推定トルクと予め定めた許容トルクとの比である関節にかかるトルクの制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００８８】
【数１４】
0005896789.tif 000015
を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定トルクは、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ）、各通過速度の１階微分値φ’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ））に基づいて計算される。
【００８９】
また、制約条件違反率計算処理部３６８は、各補間教示点におけるロボットアーム２００の先端の推定先端速度と予め定めた許容先端速度との比である手先速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００９０】
【数１５】
0005896789.tif 000016
を計算する。ここで、推定先端速度はロボットアーム２００の先端の速度ベクトルの大きさである。つまり、推定先端速度は、
【００９１】
【数１６】
0005896789.tif 000017
である。各推定先端速度は、式（７）を用いて、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づき計算される。
【００９２】
また、制約条件違反率計算処理部３６８は、各補間教示点におけるロボットアーム２００の先端の推定回転速度と予め定めた許容回転速度との比である手先回転速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、
【００９３】
【数１７】
0005896789.tif 000018
を計算する。ここで、推定回転速度は手先の回転速度ベクトルの大きさである。つまり、推定回転速度は、
【００９４】
【数１８】
0005896789.tif 000019
である。各推定回転速度は、式（７）を用いて、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づき計算される。
【００９５】
説明のため、以上の制約条件違反率を
【００９６】
【数１９】
0005896789.tif 000020
で表す。ここで、ｎは制約条件違反率の種類を表す。本第１実施形態では、６＋６＋６＋６＋１＋１で２６個である。これら制約条件違反率を用い、制約条件の所定制約範囲を式（８）のように設定する。
【００９７】
【数２０】
0005896789.tif 000021
【００９８】
制約を満足するためには制約条件違反率αｎ（ｓ）が±１の範囲に入っている必要がある。つまり、次の２つの式を両方満足する必要がある。
【００９９】
また、ロボットアーム２００が教示点の始点から終点まで移動するのに要する所要時間は、各通過速度の逆数を変数ｓで積分し、次の式（９）の目的関数で求まる。
【０１００】
【数２１】
0005896789.tif 000022
【０１０１】
通過速度をパラメタとし、式（９）の目的関数を、式（８）の不等式制約を満足する範囲で最小化する問題は、不等式制約最適値問題として知られており、様々な解法が提案されている。例えば、次のようなラグランジェ関数を最小化する方法である。
【０１０２】
【数２２】
0005896789.tif 000023
【０１０３】
ここで、ρはペナルティパラメタと呼ばれている。
【０１０４】
本第１実施形態では、最短時間制御処理部３６９は、制約条件違反率αｎ（ｓ）が±１の範囲内となる制約条件の下、各通過速度を変更して、式（９）の目的関数を最小化する最短時間制御処理を実行する（最短時間制御処理工程）。
【０１０５】
具体的には、まず、最短時間制御処理部３６９の収束判定処理部３７０が、次の２つの条件を両方満たしたとき収束完了と判定する。
【０１０６】
条件１として、全ての制約条件違反率αｎ（１），αｎ（２），…，αｎ（３１）が±１以内であること。つまり式８を満足すること。
【０１０７】
条件２として、通過速度分布φ（１），φ（２），…φ（３１）が前回の通過速度分布φ０（１），φ０（２），…，φ０（３１）と比較して、予め定めた微少量ε以内の変化であること。つまり、以下の式（１１）を満たすこと。
【０１０８】
【数２３】
0005896789.tif 000024
【０１０９】
なお、収束判定処理部３７０は、前回の通過速度分布φ０（１），φ０（２），…，φ０（３１）のデータがない場合（つまり反復処理の１順目）は、収束判定に、条件１のみを用いて判定する。
【０１１０】
この収束判定処理部３７０で、収束していないと判定した場合、最短時間制御処理部３６９の通過速度変更処理部３７１は、式（１０）に基づき、各通過速度の値を変更する処理を実行する。そして、通過速度微分係数計算処理部３６４、推定値計算処理部３６５、制約条件違反率計算処理部３６８は、変更された各通過速度を用いて上述した各演算処理を実行する。これらの反復処理は、収束判定処理部３７０で収束したと判定するまで実行される。以上のように、最短時間制御処理部３６９は、式（９）の目的関数を、式（８）の不等式制約条件を満足する範囲で最小化するよう、通過速度を最適化する。
【０１１１】
次に、収束判定処理部３７０が収束判定で収束したと判定した場合、通過速度関数計算処理部３７２は、最短時間制御処理で得られた各通過速度から通過速度関数を求める通過速度関数計算処理を実行する（通過速度関数計算処理工程）。
【０１１２】
次に、位置指令生成処理部３７３は、求められた通過速度関数を所定時間間隔δの値で時間積分する度に、該積分の値を補間関数における変数ｓに代入することで、位置指令を生成する位置指令生成処理を実行する（位置指令生成処理工程）。
【０１１３】
図５に位置指令生成処理部３７３で求める位置指令の例を示す。この位置指令生成処理部３７３は、通過速度関数φ（ｓ）を予め定めた所定時間間隔δ（例えば１ｍｓ）ごとに積分して教示点番号ｓを求め、その位置における関節値を示す位置指令を補間関数を使って計算する。ただし、第１補間部３５１と違って、微分係数の計算は不要である。この計算は、最も簡単な積分公式を使うと、次のように簡潔に記述できる。
【０１１４】
【数２４】
0005896789.tif 000025
【０１１５】
【数２５】
0005896789.tif 000026
【０１１６】
ここで、添え字のｉは所定時間間隔δごとに計算する指令値の番号である。また、変数ｓの初期値はゼロ、つまり
【０１１７】
【数２６】
0005896789.tif 000027
とする。
【０１１８】
位置指令生成処理部３７３は、式（１２）で所定時間間隔δ後の教示点の位置ｓｉ＋１を計算し、式（１３）で各関節の位置指令（ベクトル）ｑｉ＋１を計算する。こうして求まった、一定時間間隔ごとの位置指令ｑｉは、式（８）、式（９）で記述した不等式制約最適値問題の解となっているので、ロボットアーム２００は最短時間で動作する。
【０１１９】
以上、本第１実施形態では、各補間教示点における補間関数の変数ｓに対する各通過速度φ（１），φ（２），…φ（３１）を目的関数が最小化するように変更する最短時間制御処理を行う。そして、このようにして求められた各通過速度φ（１），φ（２），…φ（３１）を用いて、補間関数から所定時間間隔毎の位置指令ｑ１，ｑ２，…を生成している。ここで、第１補間部３５１により、最短時間制御に使用する補間教示点の数が決まる。一般に不等式制約最適値問題は計算時間がかかるが、補間教示点の点数は、最終的に生成する、サーボ制御装置２３０に等時間間隔で送る位置指令の点数に対して少なく設定できる。例えば、補間教示点の点数を３１、位置指令の点数を１００００とすると、３１／１００００、すなわち約０．３％の点数となるので格段に計算量を少なくすることができる。
【０１２０】
このように、本第１実施形態では、位置指令ｑ１，ｑ２，…を、目的関数の最小化を行った後に求めているので、補間教示点の数を位置指令の数と同数にする必要はなく、補間教示点の数を、位置指令の数よりも少なくすることが可能である。そのため、最短時間制御処理に要する計算量を低減することができ、従来よりもＣＰＵ３０１において位置指令を求める計算時間を短縮して、計算速度を向上させることができる。
【０１２１】
また、各補間教示点の微分係数は、微分係数計算処理部３６２で一度計算するだけで、通過速度計算部３５２では定数として扱えるため、計算量が少なく、計算時間はそのぶんだけ少ない。
【０１２２】
また、従来技術では、制約条件が、ロボットアームの関節の推定速度の制約条件違反率が所定制約条件範囲内となる条件のみであった。これに対し、本第１実施形態では、制約条件には、ロボットアーム２００の関節の推定速度、推定加速度、推定加加速度、推定トルク、推定手先速度および推定手先回転速度の制約条件違反率が式（８）で示した所定制約条件範囲内となる条件が含まれている。したがって、従来技術に対して幅広い制約条件に対応できており、ロボットアーム２００の動作が安定する。例えば、制約条件に、推定加加速度と予め定めた許容加加速度との比が所定制約範囲内となる条件が含まれるので、ロボットアーム２００に異常な加振力がかるのを抑制することができ、ロボットアーム２００が大きく振動するのを抑制することができる。
【０１２３】
なお、本第１実施形態では、補間関数として３次スプラインを例として説明したが、４次以上のスプライン補間、あるいは多項式補間により補間関数を求めてもよい。さらに、直線補間、円弧補間などにより補間関数を求めてもよい。
【０１２４】
また、本第１実施形態では、複数の教示点から一定時間ごとの位置指令を作成するまでを連続した一連の動作として説明したが、次のように途中で２段に分けて行ってもよい。
【０１２５】
計算処理Ａとして、第１補間部３５１と通過速度計算部３５２との処理を連続して行い、通過速度φ（ｓ）を求める。
【０１２６】
計算処理Ｂとして、第２補間部３５３（位置指令生成処理部３７３）により、通過速度φ（ｓ）と教示点から一定時間ごとの位置指令を作成する。
【０１２７】
計算処理Ａは、最適化計算であり、計算量の多いので、ロボットアーム２００を動作させる前に計算しておく。後半の計算処理Ｂは、計算量が少ないので、ロボットアーム２００を実際に動かしながら処理することが可能である。この場合、計算のためにロボットアーム２００が停止する時間は、計算処理Ａの処理時間だけとなる。
【０１２８】
つまり、計算処理Ａ，Ｂが全部終わってからロボットアーム２００を動かす方法よりも、ロボットアーム２００の停止時間を短縮できる。
【０１２９】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット制御装置について説明する。図６は、本発明の第２実施形態に係るロボット制御装置の微分係数計算処理部の動作を説明するための模式図である。なお、本第２実施形態では、微分係数計算処理部３６２（図３）の計算処理のみが上記第１実施形態と異なる。したがって、本第２実施形態では、微分係数計算処理部３６２の計算処理について説明する。
【０１３０】
上記第１実施形態では、関節空間移動（Ｍｏｔｉｏｎ  ｉｎ  ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅ）の場合を例に説明した。本第２実施形態ではタスク空間移動（Ｍｏｔｉｏｎ  ｉｎ  ｔａｓｋ  ｓｐａｃｅ）の場合を説明する。本第２実施形態では、上記第１実施形態とは、式（１）に示した補間関数が異なる。
【０１３１】
タスク空間移動の場合、補間関数はタスク空間で表現されている教示点を補間した後、ロボットアーム２００の逆機構学（ｉｎｖｅｒｓｅ  ｋｉｎｅｍａｔｉｃｓ）計算を行って関節空間に変換し、補間教示点を求める。この時、複雑な逆機構学計算を行うため、解析的に微分係数を計算することが困難である。
【０１３２】
本第２実施形態では、上記第１実施形態と同様に、演算部としてのＣＰＵ３０１（図２）は、プログラム３３０に基づき、微分係数計算処理部３６２として機能する。
【０１３３】
そして、本第２実施形態では、微分係数計算処理部３６２が、補間関数が解析的に微分困難な場合に対応するため数値微分する。ただし、本第２実施形態では、粗く補間することによって補間教示点の点数を減らしているので、単純に補間教示点間の値で数値微分すると、補間誤差が大きくなる。そこで、補間分割したそれぞれの位置において次のように微分係数を求める。
【０１３４】
微分係数計算処理部３６２は、微分係数計算処理として、各補間教示点ｑ（１）〜ｑ（３１）の各ベクトル成分に対して変数ｓを各補間教示点の間隔よりも小さい微小値ｅで変化させたときの各ベクトル成分を補間関数に基づいて計算する。そして、微分係数計算処理部３６２は、該計算の結果を用いて、各微分係数を計算する。具体的には、微分係数計算処理部３６２は、各補間教示点ｑ（１）〜ｑ（３１）におけるベクトル成分の１階微分値、２階微分値、３階微分値を計算する。
【０１３５】
詳述すると、まず、微分係数計算処理部３６２は、ｓ＝１，２，…，３１における補間関数の値、及び変数ｓを微小値ｅずらした位置での補間関数の値を、それぞれのｓの値について４点計算する。
【０１３６】
【数２７】
0005896789.tif 000028
【０１３７】
次に、微分係数計算処理部３６２は、次式（１５）で１階微分値、２階微分値、３階微分値を計算する。
【０１３８】
【数２８】
0005896789.tif 000029
【０１３９】
上式（１５）は一つの例であり、数値微分の公式はこの他にも多数知られている。この方法によれば、解析的に微分することが困難な補間関数であっても、１階微分値、２階微分値、３階微分値を精度よく計算でき、最短時間制御を実施できる。
【０１４０】
［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について説明する。本第３実施形態では、ロボット装置１００Ｂは、複数の多関節ロボットとして、２つのロボットアーム２００ａ，２００ｂと、これらロボットアーム２００ａ，２００ｂを制御するロボット制御装置３００Ｂと、を備えている。なお、本第３実施形態では、演算部としてのＣＰＵの計算処理において、上記第１、第２実施形態とは補間関数を求める補間関数計算処理が異なるものであるが、他の計算処理は、上記第１、第２実施形態と同様である。
【０１４１】
したがって、本第３実施形態では、ロボット制御装置３００Ｂは、２つのロボットアーム２００ａ，２００ｂを制御対象としているが、それぞれのロボットアームにおいて、補間関数計算処理以外は、上記第１、第２実施形態と同様の処理を行うものである。
【０１４２】
人間の組立では、両方の腕や手など、複数要素が同期して動く。同じようにロボットアームのロボット制御装置においても、複数のロボットアームの同期動作を実現できなければならない。
【０１４３】
本第３実施形態では、上記第１、第２実施形態と同様に、演算部としてのＣＰＵ３０１（図２）は、プログラム３３０に基づき、補間関数計算処理部３６１として機能する。
【０１４４】
そして、本第３実施形態では、補間関数計算処理部３６１は、補間関数計算処理として、各教示点に、時刻に対応した教示点番号を付与し、該教示点番号を用いたスプライン補間により補間関数を求める。
【０１４５】
具体的に説明すると、補間関数計算処理部３６１は、それぞれの教示点列の教示点に単調増加する教示点番号を設定し、その教示点番号を使った不等間隔スプラインによって教示点列の教示点間を補間する補間関数を求める。
【０１４６】
例えば、図７（ａ）に示すように、第１のロボットアーム２００ａに対しては、教示点が４つ（１ｐ１，１ｐ２，１ｐ３，１ｐ４）、第１のロボットアーム２００ａに対しては、教示点が３つ（２ｐ１，２ｐ３，２ｐ４）あるものとする。これらの教示点で、後半の２つは両方のロボットアーム２００ａ，２００ｂを協調させて作業するため同時刻に通過させたいとする。
【０１４７】
それぞれの教示点列において単調増加する教示点番号は１番からスタートするが、第２のロボットアーム２００ｂについては２番が抜けている。２つの教示点列において、同じ教示点番号のところで同期した動きを実現するのが目的である。つまり、２番を除いた１番、３番、４番の教示点をそれぞれのロボットアーム２００ａ，２００ｂが同時に通過することが目的である。
【０１４８】
これは、補間関数を不等間隔スプライン関数にすることによって実現できる。不等間隔スプライン関数は例えば、Ｂスプラインなどが知られている。
【０１４９】
【数２９】
0005896789.tif 000030
【０１５０】
【数３０】
0005896789.tif 000031
とする。両者の要素を合体し、改めて補間関数として次のように表記する。
【０１５１】
【数３１】
0005896789.tif 000032
【０１５２】
これ以降の処理は、上記第１、第２実施形態と同様である。
【０１５３】
本第３実施形態では、上記第１及び第２実施形態の効果に加えて、教示番号が同じところ、上式では同じｓの値のところでロボットアーム２００ａ，２００ｂを同期させることができる。つまり複数のロボットアームの同期動作に対応した最短時間制御を実現できる。
【０１５４】
［第４実施形態］
次に、本発明の第４実施形態のロボット制御装置について説明する。図８は、本発明の第４実施形態に係るロボット制御装置における制約条件違反率計算処理部の処理動作を説明するための図である。
【０１５５】
本第４実施形態のロボット制御装置では、予め定めておいた制約値（許容値）が、補間教示点ごとに異なる値に設定されている。あらかじめ異なる制約値を教示点ごとに与え、その値に従って式（８）を用いて制約条件違反率を計算する。その他の計算処理は上記第１〜第３実施形態と同様であるので、説明を省略する。
【０１５６】
上記第１〜第３実施形態の制約条件違反率計算処理部３６８では、制約条件違反率として、推定値と予め定めた許容値との比を計算していた。例えば第ｊ軸の関節の速度の場合、上記第１〜第３実施形態では、関節の推定速度と定数である許容速度Ｖｊとの比、
【０１５７】
【数３２】
0005896789.tif 000033
を計算していた。
【０１５８】
本第３実施形態では、許容速度が変数の値に応じて変化する値に設定され、推定速度と教示点ごとに異なる許容速度との比、
【０１５９】
【数３３】
0005896789.tif 000034
が計算される。
【０１６０】
図８示す横軸は補間教示点の番号を示す変数ｓであり、変数ｓは補間するので整数に限らず、連続した数字である。この図８は、通過速度の最適化計算が完了した後を図示している。従って関節の推定速度は許容速度を越えていない。
【０１６１】
本第４実施形態では、教示点ごとに制約条件を設定できるので、上記第１〜第３実施形態よりもさらに制約条件の自由度が向上する。
【０１６２】
更に、本第４実施形態では、図８に示すように、許容速度が、変数ｓの値が増加するに連れて小さくなるように設定されている。これにより、ロボットアームが終点に近づくに連れて関節の速度が低下するので、ロボットアームを用いた組立作業を安定して行うことができる。
【０１６３】
なお、本第４実施形態では、関節の速度の制約について説明したが、関節の加速度や加加速度、トルク、手先速度、手先回転速度など他の制約条件についても同じように、教示点ごとに許容値を設定できる。
【０１６４】
以上説明したように、本第４実施形態によれば、上記第１〜第３実施形態の効果に加えて、教示点ごとに異なる許容値を設定してロボットアームの最短時間制御を実現できる。
【０１６５】
［第５実施形態］
次に、本発明の第５実施形態に係るロボット制御装置について説明する。本第５実施形態では、制約条件には、上記第１〜第４実施形態における条件の他、更に、推定モータトルクと予め定めた許容モータトルクとの比が所定制約範囲内（±１の範囲内）となる条件が含まれている。
【０１６６】
ロボットアーム２００の関節Ｊ１〜Ｊ６と電動モータ２１１〜２１６は減速機２２１〜２２６を介して接続されている。この減速機２２１〜２２６と電動モータ２１１〜２１６との計算モデルから各電動モータ２１１〜２１６における推定モータトルクが計算できる。
【０１６７】
図９（ａ）に電動モータ２１１〜２１６のモデルを示す。ここで次の記号を使った。
Ｎｊ    第ｊ軸の減速機の減速比
τｊ    第ｊ軸モータの推定モータトルク
Ｉｊ    第ｊ軸モータの慣性モーメント
ｃｊ    第ｊ軸の粘性要素
ｄｊ    第ｊ軸の摩擦要素
【０１６８】
このモデルを具体的に式で表現する。制約条件を計算するときに使った関節の推定速度、推定加速度および関節にかかる推定トルクを用いる。電動モータ２１１〜２１６の推定モータトルクは、減速比Ｎｊを考慮して次のように書ける。
【０１６９】
【数３４】
0005896789.tif 000035
【０１７０】
右辺第１項は式６で計算した関節にかかるトルクである。第２項はモータの慣性力、加速度に比例した成分である。第３項はモータの粘性力、速度に比例した成分である。第４項はモータの摩擦力、速度の符号に応じて、符号が変わる成分である。
【０１７１】
推定値計算処理部３６５は、更に、各補間教示点ｑ（ｓ）における電動モータ２１１〜２１６の推定モータトルクを、減速機２２１〜２２６の減速比、各通過速度及び各微分係数に基づいて計算する。なお、この場合、推定トルクｕｊ（ｓ）は、式（４）、式（５）及び式（６）から求めることができるので、各補間教示点、各通過速度（通過速度、通過速度の１階及び２階微分値）及び各微分係数（補間関数の１階、２階微分値）に基づいて計算されることとなる。
【０１７２】
この式（１７）から明らかなように、左辺のモータのトルクは、第１項の関節にかかるトルクよりも大きい。つまり減速機の慣性や粘性などの影響をうけて、モータには、より大きなトルクが必要である。
【０１７３】
このようにして計算された推定モータトルクは、許容モータトルクを超えてはならない。許容モータトルクは、一般に、横軸にモータの速度、縦軸にモータのトルクをとったＴＮ曲線と呼ばれるグラフで表現される。この推定モータトルクと許容モータトルクとの比についても制約条件違反率の計算に加えることによってモータトルクも考慮した最短時間制御が実現できる。
【０１７４】
ＴＮ曲線の例を図９（ｂ）に示す。このように許容モータトルクは一般に速度の関数である。図３で説明した制約条件違反率計算処理部３６８に次の制約条件違反率の計算を追加する。
【０１７５】
【数３５】
0005896789.tif 000036
【０１７６】
６軸多関節ロボットの場合ｊ＝１〜６種類ある。
【０１７７】
図９（ｂ）では、推定モータトルクが、許容範囲にすべて入っている状況を示している。つまり上記の制約条件違反率の大きさが１以下となっている。こうして制約条件違反率を計算する以外の部分は、上記第１〜第４実施形態と同様であり説明を省略する。
【０１７８】
以上説明したように、本第５実施形態によれば、上記第１〜第４実施形態の効果に加え、モータトルクについても制約条件を設定できるので、第１の発明よりもさらに制約条件の自由度を向上できる。そして、許容モータトルクの制約条件を満たしたロボットアームの最短時間制御を実現できる。
【０１７９】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。
【０１８０】
上記第１〜第５実施形態では、全ての種類の制約条件を考慮したが、これらの制約条件を常に考慮する必要はない。例えば推定手先速度や推定手先回転速度に関係なく、動作速度を優先した動作は、推定手先速度や推定手先回転速度の制約条件をαｎ（ｓ）の種類からはずすだけで、実現できる。このようにどの制約の種類を使うかは予め定めておけばよい。その際、微分係数計算処理部３６２は、必要な微分係数のみ計算するようにすればよい。また、通過速度微分係数計算処理部３６４、推定値計算処理部３６５においても、必要な微分値、推定値のみ計算するようにすればよい。特に、推定速度のみ計算する場合は、通過速度微分係数計算処理部３６４は省略可能である。
【０１８１】
また、上記第１〜第５実施形態では、ロボット制御装置の制御対象である多関節ロボットがロボットアームの場合について説明したが、多関節ロボットがロボットアームの先端に設けられるロボットハンドであっても適用可能である。
【０１８２】
また、以上述べた実施形態の各処理動作は具体的にはロボット制御装置の演算部としてのＣＰＵ３０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体をロボット制御装置３００に供給し、ロボット制御装置３００のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。
【０１８３】
また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０が格納される場合について説明したが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図２に示すＨＤＤ３０４、外部記憶装置３２２、記録ディスク３３１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。
【０１８４】
また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。
【０１８５】
また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。
【０１８６】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。
【符号の説明】
【０１８７】
１００…ロボット装置、２００…ロボットアーム（多関節ロボット）、２０１〜２０６…アクチュエータ、２１１〜２１６…電動モータ、２２１〜２２６…減速機、２３０…サーボ制御装置（駆動制御部）、３００…ロボット制御装置、３３０…プログラム、３３１…記録ディスク（記録媒体）、３６１…補間関数計算処理部（演算部）、３６２…微分係数計算処理部（演算部）、３６３…仮設定処理部（演算部）、３６５…推定値計算処理部（演算部）、３６８…制約条件違反率計算処理部（演算部）、３６９…最短時間制御処理部（演算部）、３７２…通過速度関数計算処理部（演算部）、３７３…位置指令生成処理部（演算部）
【図１】
0005896789.tif 000037
【図２】
0005896789.tif 000038
【図３】
0005896789.tif 000039
【図４】
0005896789.tif 000040
【図５】
0005896789.tif 000041
【図６】
0005896789.tif 000042
【図７】
0005896789.tif 000043
【図８】
0005896789.tif 000044
【図９】
0005896789.tif 000045