JP2015051469A - ロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体 - Google Patents

Abstract

【課題】最短時間制御に要する計算量を低減すると共に、教示点の始点から終点までの多関節ロボットの動作時間を短縮する。
【解決手段】補間関数計算処理部３６１は、複数の教示点における始点から終点までの間を複数の区間に区切り、各区間にそれぞれ指定された補間方法で、区間毎に補間教示点の通過順番を変数ｓとした区間補間関数をそれぞれ求める。この際、区間補間関数は、タスク空間及び関節空間のうちいずれか一方に統一して求める。補間関数計算処理部３６１は、各区間補間関数を、区間補間関数の番号ｋにより切り替えることにより結合して全区間に跨る、変数ｓを用いた補間関数を求める。
【選択図】図３

Description

本発明は、教示点で与えられる軌道に沿って多関節ロボットを動作させるとき、動作時間が最短になる位置指令を生成するロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体に関する。

近年、人間の手のように複雑で高速な組立を実現できる、多関節ロボットを有する組立装置の開発事例が増えており、多関節ロボットの高速動作が重要となっている。

しかも、複雑で精密な組立て対象に対応するためには、２つの教示点間を結ぶ単純なＰＴＰ（Point to Point）軌道ではなく、３つ以上の教示点を結ぶ複雑なＣＰ（Continuous Path）軌道に対応する必要がある。

このような要求に応えるべく、多関節ロボットの最短時間制御（Time Optimal Control）の技術が提案されている（特許文献１参照）。この最短時間制御は、１９８０年代から論文が散見され、よく知られている。特許文献１では、多関節ロボットの各関節に対する位置指令（目標位置）を所定時間間隔（例えば１ｍｓ間隔）で出力するために、与えられた各教示点間を所定時間間隔で補間して位置指令を求めている。そして、各位置指令の値を、動作時間が最小化するように修正している。

また、教示点を指定した際には、各教示点間の補間方法を指定するのが一般的である。補間方法としては直線補間（Linear Interpolation）、円弧補間（Circular Interpolation）、関節補間（Joint Interpolation）などがあげられる。

ロボットを用いた組立を行う際には、複数の種類の補間方法を使い分けることが多い。例えば、第１教示点から第２教示点までの第１区間では直線補間を指定し、第２教示点から第３教示点までの第２区間では関節補間を指定するといったことが行われる。例えば、ロボットアームの先端（ロボットハンド）に直進性が必要な嵌合作業時には直線補間を使用し、直線補間が使えない特異点付近では関節補間を用いるというようにユーザが使い分けを行う。

特開平１１−１９８０７２号公報

しかしながら、上記特許文献１では、最短時間制御に要する計算量が多い。具体的に説明すると、上記特許文献１では、各教示点に基づき目標位置を示す位置指令を計算し、この計算した位置指令に対して最短時間制御の演算を行い、修正した位置指令を生成している。

位置指令は、多関節ロボットの速度や位置精度が低下するのを抑制するために、微小（例えば数ミリ秒）の所定時間間隔で出力する必要があり、データ量が多い。例えば多関節ロボットを１０秒間動かす際に、１ｍｓ間隔で位置指令を出力する際には、１０×１０００＝１万個の修正した位置指令が必要となる。従って、最短時間制御で演算処理するデータ量が多くならざるを得ない。そして、最短時間制御は、最適化問題に帰着して解くものであり、計算時間を要するものであり、データ量の増加は計算時間の増加となる。演算処理している間は、多関節ロボットを動作させることができないので、最短時間制御の目的であった、多関節ロボットの高速動作ができていないという問題があった。

また、区間毎に個別に補間関数を設定し、区間毎に最適化計算を行って多関節ロボットの軌道を生成すると、補間関数が切り替わる境界で多関節ロボットが一時停止するなど滑らかに動作しないことがあった。これにより、教示点の始点から終点までの全区間に要する多関節ロボットの動作時間が長くなっていた。

そこで、本発明は、最短時間制御に要する計算量を低減すると共に、教示点の始点から終点までの間において上記境界で多関節ロボットが滑らかに動作することで動作時間を短縮することを目的とする。

本発明は、多関節ロボットが、指定された複数の教示点を辿って動作するよう、前記多関節ロボットの各関節を駆動する各アクチュエータの動作を位置指令に基づいて制御する駆動制御部に、位置指令を出力するロボット制御装置において、前記複数の教示点における始点から終点までの間を複数の区間に区切り、前記多関節ロボットの位置及び姿勢を規定するタスク空間、及び前記多関節ロボットの各関節角度を規定する関節空間のうち、いずれか一方に統一して、前記各区間にそれぞれ指定された補間方法で区間補間関数をそれぞれ求める区間計算処理と、前記各区間補間関数を結合して全区間に跨る、補間教示点の通過順番を示す変数を用いた補間関数を求める補間関数計算処理と、前記補間関数から求められる各補間教示点を前記変数で微分した微分係数をそれぞれ計算する微分係数計算処理と、前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理と、前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理と、前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記教示点の始点から終点に到達する時間を最小化する最短時間制御処理と、前記最短時間制御処理で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理と、前記通過速度関数から前記位置指令を生成する位置指令生成処理と、を実行する演算部を備えたことを特徴とする。

本発明によれば、各補間教示点における補間関数の変数に対する各通過速度を目的関数が最小化するように変更する最短時間制御処理を行い、このようにして求められた各通過速度を用いて、補間関数から所定時間間隔毎の位置指令を生成している。このように、位置指令は目的関数の最小化を行った後に求めているので、補間教示点の数を位置指令の数と同数にする必要はなく、補間教示点の数を、位置指令の数よりも少なくすることが可能である。したがって、最短時間制御処理に要する計算量を低減することができ、演算部において位置指令を求める計算時間を短縮させることができる。

また、複数の補間方法が混在して多関節ロボットの軌道を生成する際にも、最短時間制御処理前に複数の区間補間関数を１つの補間関数に結合してから最短時間制御処理を行っている。そのため、補間方法毎に個別に最適化した時と比べて、区間補間関数の境界を跨いでも滑らかに動作することが可能になり全体での動作時間の短縮が可能となる。

第１実施形態に係るロボット装置の概略構成を示す説明図である。 ロボット制御装置の構成を示すブロック図である。 ロボット制御装置の制御動作を示すフローチャートである。 多関節ロボットの軌道の計算過程を説明するための模式図である。 補間関数計算処理部の演算処理を示すフローチャートである。 複数の教示点の始点から終点までに要する時間を示すグラフである。 第２実施形態に係るロボット制御装置による多関節ロボットの軌道の計算過程を説明するための模式図である。 第３実施形態に係るロボット制御装置による多関節ロボットの軌道の計算過程を説明するための模式図である。 第４実施形態に係るロボット制御装置の微分係数計算処理部の動作を説明するための模式図である。 第５実施形態に係るロボット装置の概略構成及び各ロボットアームの教示点を示す説明図である。 第６実施形態に係るロボット制御装置における制約条件違反率計算処理部の処理動作を説明するための図である。 第７実施形態に係る電動モータのモデル及びＴＮ曲線の説明図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。図１に示すロボット装置１００は、多関節ロボットとしてのロボットアーム２００と、ロボットアーム２００を制御するロボット制御装置３００と、を備えている。また、ロボット装置１００は、ロボット制御装置３００に複数の教示点のデータを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、人が操作するものであり、ロボットアーム２００やロボット制御装置３００の動作を指定するのに用いる。

ロボットアーム２００は、第１実施形態では、例えば６関節ロボットである。ロボットアーム２００は、各関節Ｊ１〜Ｊ６を各関節軸Ａ１〜Ａ６まわりにそれぞれ回転駆動する複数（６つ）のアクチュエータ２０１〜２０６を有している。ロボットアーム２００は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に手先（ロボットアーム２００の先端）を向けることができる。一般に、ロボットアーム２００の位置と姿勢は、座標系で表現することができる。図１中の符号Ｔｏは、ロボットアーム２００の台座に固定した座標系を表し、符号Ｔｅはロボットアーム２００の手先に固定した座標系を表す。

第１実施形態では、各アクチュエータ２０１〜２０６は、電動モータ２１１〜２１６と電動モータ２１１〜２１６に接続された減速機２２１〜２２６とからなる。なお、各アクチュエータ２０１〜２０６の構成は、これに限定するものではなく、例えば人工筋肉等であってもよい。

第１実施形態では、各関節Ｊ１〜Ｊ６が回転関節である場合について説明する。この場合、「関節の位置」とは、当該関節に定められた基準からの関節の回転角度を意味する。なお、各関節が直動関節の場合であってよい。その場合、「関節の位置」とは、直動関節の当該関節に定められた基準から位置である。同様に、それらの時間微分についても「関節の速度」、「関節の加速度」、「関節の加加速度」という。

ロボットアーム２００は、更に、各アクチュエータ２０１〜２０６の電動モータ２１１〜２１６を駆動制御する駆動制御部としてのサーボ制御装置２３０を有している。サーボ制御装置２３０は、入力した位置指令（目標位置）に基づき、各関節Ｊ１〜Ｊ６の位置が位置指令に追従するよう、各電動モータ２１１〜２１６に電流を出力し、各電動モータ２１１〜２１６の動作を制御する。

ロボット制御装置３００は、ティーチングペンダント４００から複数の教示点（教示点列）の入力を受ける。ロボット制御装置３００は、ロボットアーム２００の各関節Ｊ１〜Ｊ６が、複数の教示点を順次辿って動作するよう、教示点に基づき、サーボ制御装置２３０に所定時間間隔で出力する位置指令を生成し、所定時間間隔で位置指令を出力する。

教示点は、各関節（６つの関節）Ｊ１〜Ｊ６の教示位置をベクトル成分として含むベクトル（教示点ベクトル）である。そして、ロボット制御装置３００において最終的に求められる位置指令は、各関節（６つの関節）Ｊ１〜Ｊ６の目標位置をベクトル成分として含むベクトルである。第１実施形態では、教示点と位置指令とは、点数は異なるが、次元（単位）は同一である。

つまり、ロボット制御装置３００は、入力した教示点からＣＰ軌道の計算をして軌道に沿った多数の位置指令を生成し、これら位置指令を所定時間間隔でサーボ制御装置２３０に出力するものである。

その際、ロボット制御装置３００は、与えられた教示点列によって定まる軌道に対し、軌道上の通過速度を、ロボットアーム２００の各関節Ｊ１〜Ｊ６の速度、加速度、トルク等の制約条件を越えない範囲で調整する、多関節ロボットの最短時間制御を行う。

ここで、教示によるロボットアーム２００の動作方法として次の２種類が考えられる。

１ 関節空間移動 Motion in configuration space
ロボットの関節角度を変位とする座標系を関節空間という。前記関節空間でロボットアーム２００の教示点を指定し、指定した教示点で決定される軌道に沿ってロボットを動かす方法。

２ タスク空間移動 Motion in task space
三次元的な位置と姿勢で表す座標系をタスク空間という。前記タスク空間で教示点を指定し、指定した教示点とロボットアーム２００の手先で決定される軌道に沿ってロボットアーム２００を動かす方法。

図１では、４つの教示点ｐ_１，ｐ_２，ｐ_３，ｐ_４を模式的に図示している。いずれの場合でもロボットアーム２００の動作は教示点で決まる軌道に沿って動く。ここで、教示点はいわゆる３次元空間内の点ではない。関節空間移動の場合では、教示点は全関節Ｊ１〜Ｊ６の関節角度（全モータの回転角度）である。タスク空間移動の場合では、教示点は３次元空間の位置および姿勢であり、これは図１のように、ｐ_１，ｐ_２，ｐ_３，ｐ_４を座標系として表現される。

以下、第１実施形態の具体的な実施形態を説明する。

図２は、ロボット制御装置３００の構成を示すブロック図である。第１実施形態では、ロボット制御装置３００は、図２に示すように、演算部としてのＣＰＵ（Central Processing Unit）３０１を備えたコンピュータである。また、ロボット制御装置３００は、記憶部としてＲＯＭ（Read Only Memory）３０２、ＲＡＭ（Random Access Memory）３０３、ＨＤＤ（Hard Disk Drive）３０４を備えている。また、ロボット制御装置３００は、記録ディスクドライブ３０５及び各種のインタフェース３０６〜３０９を備えている。

ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及び各種のインタフェース３０６〜３０９が、バス３１０を介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果を一時的に記憶する記憶部である。ＨＤＤ３０４は、演算処理結果である各種のデータを記憶するための記憶部であると共に、プログラム３３０が格納されている。このプログラム３３０は、ＣＰＵ３０１に各種演算処理を実行させるためのプログラムである。ＣＰＵ３０１は、ＨＤＤ３０４に格納されたプログラム３３０に基づいて各種演算処理を実行する。

ティーチングペンダント４００はインタフェース３０６に接続されており、ＣＰＵ３０１はインタフェース３０６及びバス３１０を介してティーチングペンダント４００からの教示点のデータや補間方法の指定の入力を受ける。

ＨＤＤ３０４には、ＣＰＵ３０１による演算結果を示すデータ（位置指令のデータ）等がＣＰＵ３０１の指令の下で記憶される。

ロボットアーム２００のサーボ制御装置２３０は、インタフェース３０９に接続されており、ＣＰＵ３０１は、位置指令のデータを所定時間間隔でバス３１０及びインタフェース３０９を介してサーボ制御装置２３０に出力する。

インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３２２が接続可能に構成されている。記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。

図３は、ロボット制御装置３００の制御動作を示すフローチャートである。ＣＰＵ３０１は、ＨＤＤ３０４に格納されたプログラム３３０を実行することで、以下に説明する各処理を実行する各種の処理部３５１，３５２，３５３として機能する。つまり、ＣＰＵ３０１は、プログラム３３０に基づき、図３に示す第１補間部３５１、通過速度計算部３５２、第２補間部３５３として機能する。なお、図３において、丸四角はデータを、四角はＣＰＵ３０１の処理を表す。

各部３５１，３５２，３５３の動作について大まかに説明すると、まず、第１補間部３５１は、与えられた各教示点近傍を通過するような補間関数を求めて各教示点の間を補間し、補間教示点のそれぞれの位置において、各関節の位置を求める。更に、第１補間部３５１は、その１階微分値、２階微分値、３階微分値を計算する。

通過速度計算部３５２は、補間教示点のそれぞれの位置において通過速度を定義する。次いで、通過速度計算部３５２は、通過速度の１階微分値と、通過速度の２階微分値を計算する。通過速度計算部３５２は、それらと、第１補間部３５１で計算した各関節の位置、およびその１階微分値、２階微分値、３階微分値とから、ロボットアーム２００の各関節の、推定速度、推定加速度、推定加加速度、推定トルクを計算する。更に、通過速度計算部３５２は、ロボットアーム２００の先端の推定先端速度（以下、推定手先速度という）、およびロボットアーム２００の先端の推定回転速度（以下、推定手先回転速度という）を計算する。

次いで、通過速度計算部３５２は、計算した推定速度、推定加速度、推定加加速度、推定トルク、推定手先速度および推定手先回転速度と、予め定めておいた制約値との比である制約条件違反率を計算する。

通過速度計算部３５２は、計算した全て、または予め選択した制約条件違反率が、所定制約範囲内（±１の範囲内）となる不等式制約を満足し、通過速度の逆数の和、すなわち動作時間を最短にする、不等式制約最適化問題を解いて、通過速度を求める。

第２補間部３５３は、計算した通過速度を所定時間間隔で積分して求めた位置における、ロボットアーム２００の各関節への位置指令を、補間関数を使って計算する。これにより、第１補間部３５１、通過速度計算部３５２及び第２補間部３５３を経ることにより最適な軌道が位置指令値ｑｉとして計算される

ここで、数式における記号を以下のように定義する。なお、数式中、太文字はベクトルを示す。

以下、各部３５１，３５２，３５３の動作について詳細に説明する。第１補間部３５１は、図３に示すように、補間関数計算処理部３６１と、微分係数計算処理部３６２とを有している。通過速度計算部３５２は、仮設定処理部３６３と、通過速度微分係数計算処理部３６４と、推定値計算処理部３６５と、制約条件違反率計算処理部３６８と、最短時間制御処理部３６９と、通過速度関数計算処理部３７２と、を有している。第２補間部３５３は、位置指令生成処理部３７３からなる。推定値計算処理部３６５は、計算処理部３６６と、運動学計算処理部３６７とからなる。最短時間制御処理部３６９は、収束判定処理部３７０と、通過速度変更処理部３７１とからなる。

図４は、本発明の特徴の１つである多関節ロボットの軌道の計算過程を示す模式図、図５は、補間関数計算処理部３６１（ＣＰＵ３０１）の演算処理を示すフローチャートである。図４（ａ）は、教示点を説明するための模式図、図４（ｂ）及び図４（ｃ）は補間教示点及び補間関数を説明するための模式図、図４（ｄ）は通過速度を説明するための模式図、図４（ｅ）は位置指令を説明するための模式図である。

まず、補間関数計算処理部３６１は、始点から終点までの各教示点（ベクトル）ｐ_ｉをティーチングペンダント４００から取得する。例えば、補間関数計算処理部３６１は、図４（ａ）に示す４つの教示点ｐ_１〜ｐ_４を取得する。教示点ｐ_１は始点であり、教示点ｐ_４は終点である。図４（ａ）では、教示点ｐ_３を説明するために便宜上２つに分けて表現しているが、同一の位置である。

補間関数計算処理部３６１は、教示点の始点から終点までの間を複数の区間に区切り、各区間にそれぞれ指定された補間方法で、区間毎に補間教示点の通過順番を変数ｓとした区間補間関数をそれぞれ求める（Ｓ１：区間計算処理，区間計算工程）。

補間関数計算処理部３６１は、教示点ｐ_１〜ｐ_３の区間、教示点ｐ_３〜ｐ_４の区間に区切り、ユーザは、ティーチングペンダント４００を操作することで、各区間の補間方法を指定する。補間方法は、前述したような多関節ロボットの位置及び姿勢を規定するタスク空間で区間内の教示点間を補間する補間方法、及び前述したような多関節ロボットの各関節角度を規定する関節空間で区間内の教示点間を補間する補間方法がある。タスク空間で補間する補間方法としては、直線補間や円弧補間等がある。また、関節空間で補間する補間方法としては、関節補間等がある。ユーザは、ティーチングペンダント４００を操作することで、直線補間や円弧補間、関節補間等を指定する。図４（ａ）では、教示点ｐ_１〜ｐ_３の区間では関節補間、即ち関節空間での補間方法が指定され、教示点ｐ_３〜ｐ_４の区間では直線補間、即ちタスク空間での補間方法が指定されている例を示している。つまり、隣接する区間同士は、補間方法が互いに異なる。ステップＳ１の処理は、補間方法の数（即ち、区間の数）だけループして行う。なお、各区間の始点と終点は教示点であり、各区間における教示点の始点と終点との間で補間が行われる。

例えば、補間関数計算処理部３６１は、ステップＳ１では、２つの教示点ｐ_１〜ｐ_３の区間を補間する、図４（ｂ）に示す区間補間関数を、関節補間により求める。また、補間関数計算処理部３６１は、２つの教示点ｐ_３〜ｐ_４の区間を補間する区間補間関数を、直線補間により求める。

図４（ｂ）では、各区間補間関数を点線で模式的に表している。補間方法は、直線補間や円弧補間、関節補間など様々なものがあるが、全て、次の式（１）の区間補間関数で表現できる。

ここで、変数ｋは各区間補間関数に振られる通し番号であり、変数ｓは、整数の値とした場合、前述の各補間教示点ｑ（ｓ）において連番となる変数である。また、補間するので変数ｓは整数でなく連続した数（実数）の値を取り得る。図４（ｂ）の例では、関節補間に対応した補間関数ｈ_１（ｓ，ｐ_ｉ）と、直線補間に対応した補間関数ｈ_２（ｓ，ｐ_ｉ）が算出される。

区間補間関数は、関節補間の区間、直線補間の区間の両方を関節空間に統一して求めることが重要である。従って、補間関数計算処理部３６１は、タスク空間及び関節空間のうちいずれか一方（第１実施形態では関節空間）に統一して、各区間にそれぞれ指定された補間方法で区間補間関数をそれぞれ求める。なお、区間補間関数をタスク空間に統一して表現することも可能である。しかし、その場合はサーボ制御装置２３０へ位置指令を送信する前に、逆運動学演算を解いて各軸角度へ変換する必要がある。つまり、計算コストの観点から、区間補間関数は関節空間で表現するのがよい。

区間補間関数は変数ｓの多項式とするのが好ましく、第１実施形態では、後に３階の微分係数まで計算するので、３次多項式として、次のような式としている。

式（１）に示す多項式の各係数（ベクトル）ａ_０，ａ_１，ａ_２，ａ_３は、区間ｐ_ｉ−ｐ_ｉ＋１ごとに異なるものを選択し、第１実施形態では、区間の境界で２次の微分係数まで連続なものが選ばれる。

このように、補間関数計算処理部３６１は、式（１）に示す各係数ａ_０，ａ_１，ａ_２，ａ_３を計算して、区間補間関数を求める。

補間関数計算処理部３６１は、各区間の区間補間関数を計算した後、教示点の始点から終点までの間に複数の補間方法があるか否かを判断する（Ｓ２）。補間関数計算処理部３６１は、補間方法が１つの場合（Ｓ２：Ｎｏ）は、処理を終了し、複数の補間方法がある場合（Ｓ２：Ｙｅｓ）は、複数の区間補間関数を全区間に跨る１つの補間関数に結合する（Ｓ３：補間関数計算処理：補間関数計算工程）。

例えば図４（ｃ）のように２つに分かれている区間補間関数の接続を行う。図４（ｃ）の点線が各区間補間関数を接続した補間関数を模式的に示している。具体的には式（２）において区間補間関数の番号ｋに対応する区間補間関数ｈ_ｋ（ｓ，ｐ_ｉ）を設定し、切り替える。つまり、補間教示点ｑ（ｓ）の算出に必要な区間補間関数を区間補間関数の番号ｋによって切り替えるという処理を行う。つまり、関節補間で指定されている区間は区間関数ｈ_１（ｓ，ｐ_ｉ）を使用し、直線補間で指定されている区間はｈ_２（ｓ，ｐ_ｉ）を用いるという切り替えを行う。

前述の補間関数から導出される補間教示点ｑ（ｓ）は、後述する通過速度を求めるために用いられるものであるので、その点数を、位置指令の数ほど多くする必要はない。例えば教示点ｐ_ｉ−ｐ_ｉ＋１の間を１０分割すれば十分である。補間教示点ｑ（ｓ）の点数は、サーボ制御装置２３０に出力する位置指令の点数よりも少なく設定される。例えば、ロボットアーム２００が図４（ａ）に示した４つの教示点ｐ_１〜ｐ_４を１０秒間かけて移動するものとし、所定時間間隔１ｍｓでサーボ制御装置２３０に位置指令を出力する場合、１０÷０．００１＝１００００点の位置指令が必要である。これに対し、教示点ｐ_１〜ｐ_４間を１０分割したとすると、補間教示点ｑ（ｓ）の点数は３１点となり、位置指令の点数に対して格段に少なくできる。

次に、微分係数計算処理部３６２は、各補間教示点ｑ（１），ｑ（２），…に含まれる教示位置を示す各ベクトル成分を補間関数の変数ｓで微分した微分係数をそれぞれ計算する微分係数計算処理を実行する（微分係数計算処理工程）。

ここで、仮に補間教示点ｑ（１），ｑ（２），…の差分を計算する方法では、補間誤差が拡大され、高階微分には使えない。

第１実施形態では、３階微分まで使うので、従来技術のような単純な差分式は使わず、直接、補間関数を微分した関数を用いる。つまり、式（２）をそれぞれ、変数ｓで１階微分、２階微分、３階微分すると、次の式（３）のように計算される。

したがって、第１実施形態では、微分係数計算処理部３６２は、各補間教示点ｑ（１），ｑ（２）…における各ベクトル成分を変数ｓで微分した微分係数として、１階の微分係数、２階の微分係数、３階の微分係数を、式（３）を用いて計算する。つまり、微分係数計算処理部３６２は、各補間教示点での微分係数を、式（３）の変数ｓにｓ＝１，２…をそれぞれ代入して計算する。

次に、仮設定処理部３６３は、各補間教示点ｑ（１），ｑ（２）…において変数ｓを時間微分した微分値（１階の微分係数）を示す通過速度をそれぞれ仮設定する仮設定処理を実行する（仮設定処理工程）。各通過速度は、各補間教示点ｑ（１），ｑ（２）…において変数ｓが変化する速度を示している。

例えば、各通過速度は、図４（ｄ）に示すように、各補間教示点においてφ（１），φ（２）…が初期設定（仮設定）される。これら通過速度φ（ｓ）は、

の式で定義される。

例えば各通過速度φ（１），φ（２）…は一定の数字、例えば、

などに設定される。これは１秒かけて、補間教示点を１つ進むことを意味している。

次に、通過速度微分係数計算処理部３６４は、各補間教示点ｑ（１），ｑ（２）…における各通過速度φ（１），φ（２）…を変数ｓで微分した微分係数（第１実施形態では、１階微分、２階微分）を計算する計算処理を実行する。

この通過速度微分係数計算処理では、次式（４）で表す数式に基づき、計算処理を実行する。

次に、計算処理部３６６は、ロボットアーム２００の各関節Ｊ１〜Ｊ６の推定速度、推定加速度、推定加加速度を、各通過速度及び各微分係数に基づいて計算する推定値計算処理を実行する（推定値計算処理工程）。つまり、計算処理部３６６は、推定速度、推定加速度、推定加加速度を、各通過速度φ（ｓ）の値（これらに基づいて計算した微分係数φ’（ｓ）、φ’’（ｓ）も含む）、及び各微分係数ｑ’（ｓ），ｑ’’（ｓ），ｑ’’’（ｓ）の値に基づいて計算する。

具体的に説明すると、計算処理部３６６は、以下の式（５）に各計算結果を代入して、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）、推定加加速度ｑ（…）（ｓ）を計算する。これら推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）、推定加加速度ｑ（…）（ｓ）はベクトルである。

なお、推定加速度とは、推定速度を時間で１階微分した値であり、推定加加速度とは、推定速度を時間で２階微分した値である。

次に、運動学計算処理部３６７は、各補間教示点における各関節の推定トルク、ロボットアーム２００の推定手先速度、及びロボットアーム２００の推定手先回転速度を、各通過速度及び各微分係数に基づいて計算する推定値計算処理を実行する。

具体的に説明すると、運動学計算処理部３６７は、補間教示点（関節の教示位置）ｑ（ｓ）、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）から、関節にかかる推定トルク（ベクトル）ｕ（ｓ）を計算する。

この計算はロボットアーム２００の逆動力学計算として知られている。例えば文献、高野晴久、ロボットの運動学、２００５年６月１０日、オーム社（東京）、９１ページから引用すると、次式となる。

右辺第１項は慣性項、第２項はコリオリ項、第３項は重力項、第４項は外力項と呼ばれている。ロボットアーム２００を構成するリンクの配置や質量、慣性テンソルおよび各関節の位置ｑ（ｓ）、推定速度ｑ（・）（ｓ）、推定加速度ｑ（‥）（ｓ）から計算する。

また、運動学計算処理部３６７は、ロボットアーム２００の各関節の補間教示点ｑ（ｓ）、推定速度ｑ（・）（ｓ）から、ロボットアーム２００の推定手先速度（ベクトル）ｒ（・）（ｓ）及び推定手先回転速度（ベクトル）ｗ（・）（ｓ）を計算する。

この計算は順運動学計算として知られており、ヤコビ行列Ｔを使って簡単に求まる。例えば文献、高野晴久、ロボットの運動学、２００５年６月１０日、オーム社（東京）、４２ページから引用すると、次式となる。

このように、運動学計算処理部３６７は、関節の位置、速度、加速度、加加速度の推定値と、ロボットの各関節の受け持つ質量や慣性テンソルから、式（６）を用いて、関節にかかる推定トルクを計算する。また、運動学計算処理部３６７は、式（７）を用いて、ロボットアーム２００の推定手先速度及び推定手先回転速度を計算する。

次に、制約条件違反率計算処理部３６８は、各関節の推定速度と予め定めた許容速度との比である関節速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定速度は、式（５）を用いて、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づいて計算される。

また、制約条件違反率計算処理部３６８は、各関節の推定加速度と予め定めた許容加速度との比である関節加速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定加速度は、式（５）を用いて、各通過速度（φ（ｓ）、各通過速度の１階微分値φ’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ））に基づいて計算される。

また、制約条件違反率計算処理部３６８は、各関節の推定加加速度と予め定めた許容加加速度との比である関節加加速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定加加速度は、式（５）を用いて、各通過速度（φ（ｓ）、１階微分値φ’（ｓ）２階微分値φ’’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ）、ｑ’’’（ｓ））に基づいて計算される。

また、制約条件違反率計算処理部３６８は、各関節の推定トルクと予め定めた許容トルクとの比である関節にかかるトルクの制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。６軸多関節ロボットの場合、ｊ＝１〜６種類ある。各推定トルクは、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ）、各通過速度の１階微分値φ’（ｓ））及び各微分係数（ｑ’（ｓ），ｑ’’（ｓ））に基づいて計算される。

また、制約条件違反率計算処理部３６８は、各補間教示点におけるロボットアーム２００の先端の推定先端速度と予め定めた許容先端速度との比である手先速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。ここで、推定先端速度はロボットアーム２００の先端の速度ベクトルの大きさである。つまり、推定先端速度は、

である。各推定先端速度は、式（７）を用いて、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づき計算される。

また、制約条件違反率計算処理部３６８は、各補間教示点におけるロボットアーム２００の先端の推定回転速度と予め定めた許容回転速度との比である手先回転速度の制約条件違反率を計算する制約条件違反率計算処理を実行する。即ち、制約条件違反率計算処理部３６８は、

を計算する。ここで、推定回転速度は手先の回転速度ベクトルの大きさである。つまり、推定回転速度は、

である。各推定回転速度は、式（７）を用いて、各補間教示点ｑ（ｓ）、各通過速度（φ（ｓ））及び各微分係数（ｑ’（ｓ））に基づき計算される。

説明のため、以上の制約条件違反率を

で表す。ここで、ｎは制約条件違反率の種類を表す。これら制約条件違反率を用い、制約条件の所定制約範囲を式（８）のように設定する。

制約を満足するためには制約条件違反率α_ｎ（ｓ）が±１の範囲に入っている必要がある。つまり、次の２つの式を両方満足する必要がある。

また、ロボットアーム２００が教示点の始点から終点まで移動するのに要する所要時間は、各通過速度の逆数を変数ｓで積分し、次の式（９）の目的関数で求まる。

通過速度をパラメータとし、式（９）の目的関数を、式（８）の不等式制約を満足する範囲で最小化する問題は、不等式制約最適値問題として知られており、様々な解法が提案されている。例えば、次のようなラグランジェ関数を最小化する方法である。

ここで、ρはペナルティパラメータと呼ばれている。

第１実施形態では、最短時間制御処理部３６９は、制約条件違反率α_ｎ（ｓ）が±１の範囲内となる制約条件の下、各通過速度を変更して、式（９）の目的関数を最小化する最短時間制御処理を実行する（最短時間制御処理工程）。つまり、最短時間制御処理部３６９は、教示点の始点から終点に到達する時間を最小化する。

具体的には、まず、最短時間制御処理部３６９の収束判定処理部３７０が、次の２つの条件を両方満たしたとき収束完了と判定する。

条件１として、全ての制約条件違反率α_ｎ（１），α_ｎ（２）…が±１以内であること。つまり式（８）を満足すること。

条件２として、通過速度分布φ（１），φ（２）…が前回の通過速度分布φ_０（１），φ_０（２）…と比較して、予め定めた微少量ε以内の変化であること。つまり、以下の式（１１）を満たすこと。

なお、収束判定処理部３７０は、前回の通過速度分布φ_０（１），φ_０（２）…のデータがない場合（つまり反復処理の１順目）は、収束判定に、条件１のみを用いて判定する。

この収束判定処理部３７０で、収束していないと判定した場合、最短時間制御処理部３６９の通過速度変更処理部３７１は、式（１０）に基づき、各通過速度の値を変更する処理を実行する。そして、通過速度微分係数計算処理部３６４、推定値計算処理部３６５、制約条件違反率計算処理部３６８は、変更された各通過速度を用いて上述した各演算処理を実行する。これらの反復処理は、収束判定処理部３７０で収束したと判定するまで実行される。以上のように、最短時間制御処理部３６９は、式（９）の目的関数を、式（８）の不等式制約条件を満足する範囲で最小化するよう、通過速度を最適化する。

次に、収束判定処理部３７０が収束判定で収束したと判定した場合、通過速度関数計算処理部３７２は、最短時間制御処理で得られた各通過速度から通過速度関数を求める通過速度関数計算処理を実行する（通過速度関数計算処理工程）。

次に、位置指令生成処理部３７３は、求められた通過速度関数を所定時間間隔δの値で時間積分する度に、該積分の値を補間関数における変数ｓに代入することで、位置指令を生成する位置指令生成処理を実行する（位置指令生成処理工程）。

図４（ｅ）に位置指令生成処理部３７３で求める位置指令の例を示す。この位置指令生成処理部３７３は、通過速度関数φ（ｓ）を予め定めた所定時間間隔δ（例えば１ｍｓ）ごとに積分して教示点番号ｓを求め、その位置における関節値を示す位置指令を補間関数を使って計算する。ただし、第１補間部３５１と違って、微分係数の計算は不要である。この計算は、最も簡単な積分公式を使うと、次のように簡潔に記述できる。

ここで、添え字のｉは所定時間間隔δごとに計算する指令値の番号である。また、変数ｓの初期値はゼロ、つまり、

とする。

位置指令生成処理部３７３は、式（１２）で所定時間間隔δ後の教示点の位置ｓ_ｉ＋１を計算し、式（１３）で各関節の位置指令（ベクトル）ｑ_ｉ＋１を計算する。こうして求まった、一定時間間隔ごとの位置指令ｑ_ｉは、式（８）、式（９）で記述した不等式制約最適値問題の解となっているので、ロボットアーム２００は最短時間で動作する。

以上、第１実施形態では、各補間教示点における補間関数の変数ｓに対する各通過速度φ（１），φ（２）…を目的関数が最小化するように変更する最短時間制御処理を行う。そして、このようにして求められた各通過速度φ（１），φ（２）…を用いて、補間関数から所定時間間隔毎の位置指令ｑ_１，ｑ_２，…を生成している。ここで、第１補間部３５１により、最短時間制御に使用する補間教示点の数が決まる。一般に不等式制約最適値問題は計算時間がかかるが、補間教示点の点数は、最終的に生成する、サーボ制御装置２３０に等時間間隔で送る位置指令の点数に対して少なく設定できる。

このように、第１実施形態では、位置指令ｑ_１，ｑ_２…を、目的関数の最小化を行った後に求めているので、補間教示点の数を位置指令の数と同数にする必要はなく、補間教示点の数を、位置指令の数よりも少なくすることが可能である。そのため、最短時間制御処理に要する計算量を低減することができ、従来よりもＣＰＵ３０１において位置指令を求める計算時間を短縮して、計算速度を向上させることができる。

また、各補間教示点の微分係数は、微分係数計算処理部３６２で一度計算するだけで、通過速度計算部３５２では定数として扱えるため、計算量が少なく、計算時間はそのぶんだけ少ない。

また、従来技術では、制約条件が、ロボットアームの関節の推定速度の制約条件違反率が所定制約条件範囲内となる条件のみであった。これに対し、第１実施形態では、制約条件には、ロボットアーム２００の関節の推定速度、推定加速度、推定加加速度、推定トルク、推定手先速度および推定手先回転速度の制約条件違反率が式（８）で示した所定制約条件範囲内となる条件が含まれている。したがって、従来技術に対して幅広い制約条件に対応できており、ロボットアーム２００の動作が安定する。例えば、制約条件に、推定加加速度と予め定めた許容加加速度との比が所定制約範囲内となる条件が含まれるので、ロボットアーム２００に異常な加振力がかかるのを抑制することができ、ロボットアーム２００が大きく振動するのを抑制することができる。

図６は、複数の教示点の始点から終点までに要する時間を示すグラフである。図６に示すように、個別最適化した場合（直線補間と関節補間を別々に最適化する場合）と比べ、第１実施形態で示した通り全体最適化を行うことで、１０％の動作時間短縮が実現できる。

したがって、第１実施形態によれば、教示点に付随する補間方法が、関節補間から直線補間に切り替わる、即ち各区間で異なる場合でも、ロボットアーム２００の動作時間を短縮することができる。更に、複数の区間補間関数を個別に最適化する場合に比べ、複数の区間補間関数を１つの補間関数に結合した後に一度に全体を最適化することにより、ロボットアーム２００の動作時間を短縮することができる。

なお、第１実施形態では、複数の教示点から一定時間ごとの位置指令を作成するまでを連続した一連の動作として説明したが、次のように途中で２段に分けて行ってもよい。

計算処理Ａとして、第１補間部３５１と通過速度計算部３５２との処理を連続して行い、通過速度φ（ｓ）を求める。

計算処理Ｂとして、第２補間部３５３（位置指令生成処理部３７３）により、通過速度φ（ｓ）と教示点から一定時間ごとの位置指令を作成する。

計算処理Ａは、最適化計算であり、計算量の多いので、ロボットアーム２００を動作させる前に計算しておく。後半の計算処理Ｂは、計算量が少ないので、ロボットアーム２００を実際に動かしながら処理することが可能である。この場合、計算のためにロボットアーム２００が停止する時間は、計算処理Ａの処理時間だけとなる。

つまり、計算処理Ａ，Ｂが全部終わってからロボットアーム２００を動かす方法よりも、ロボットアーム２００の停止時間を短縮できる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット制御装置について説明する。図７は、本発明の第２実施形態に係るロボット制御装置による多関節ロボットの軌道の計算過程を説明するための模式図である。図７（ａ）は、教示点を説明するための模式図、図７（ｂ）及び図７（ｃ）は補間教示点及び補間関数を説明するための模式図、図７（ｄ）は通過速度を説明するための模式図、図７（ｅ）は位置指令を説明するための模式図である。

第２実施形態では、上記第１実施形態と異なり、図７（ａ）のように教示点に対し、上記第１実施形態と異なる補間方法の組み合わせ、即ち関節補間（関節空間移動）と円弧補間（タスク空間移動）の２種類の補間方法が指定されている。

与えられた教示点と補間方法に対して、上記第１実施形態と同様に、図３の補間関数計算処理部３６１により、図７（ｂ）に示すように、区間ごとに区間補間関数が計算される。ただし、上記第１実施形態と異なり、関節補間に対応した補間関数ｈ_１（ｓ，ｐ_ｉ）と、円弧補間に対応した補間関数ｈ_２（ｓ，ｐ_ｉ）が算出される。

区間補間関数は、関節補間の区間、円弧補間の区間の両方を関節空間に統一して求めることが重要である。なお、区間補間関数をタスク空間に統一して表現することも可能である。しかし、その場合はサーボ制御装置２３０へ位置指令を送信する前に、逆運動学演算を解いて各軸角度へ変換する必要がある。つまり、計算コストの観点から、区間補間関数は関節空間で表現するのがよい。

第２実施形態では、２つに分かれている区間補間関数を図７（ｃ）のように接続を行う。図７（ｃ）の点線が各区間補間関数を接続した補間関数を模式的に示している。具体的には式（１）において区間補間関数の番号ｋに対応する区間補間関数ｈ_ｋ（ｓ，ｐ_ｉ）を設定し、切り替える。つまり、補間教示点ｑ（ｓ）の算出に必要な補間関数を区間補間関数の番号ｋによって切り替えるという処理を行う。つまり、関節補間で指定されている区間は区間補間関数ｈ_１（ｓ，ｐ_ｉ）を使用し、円弧補間で指定されている区間はｈ_２（ｓ，ｐ_ｉ）を用いるという切り替えを行う。

以降の処理では、上記第１実施形態と同様、図７（ｄ）及び図７（ｅ）に示すように、通過速度及び位置指令が計算される。

以上説明したように、第２実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、関節補間から円弧補間に切り替わる場合にも、ロボットアーム２００の動作時間を短縮することができる。

更に、複数の区間補間関数を個別に最適化する場合に比べ、複数の区間補間関数を１つの補間関数に結合した後に一度に全体を最適化することにより、ロボットアーム２００の動作時間を短縮することができる。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット制御装置について説明する。図８は、本発明の第３実施形態に係るロボット制御装置による多関節ロボットの軌道の計算過程を説明するための模式図である。図８（ａ）は、教示点を説明するための模式図、図８（ｂ）及び図８（ｃ）は補間教示点及び補間関数を説明するための模式図、図８（ｄ）は通過速度を説明するための模式図、図８（ｅ）は位置指令を説明するための模式図である。

第３実施形態は、上記第１，第２実施形態と異なり、図８（ａ）のように教示点に対し、円弧補間（タスク空間移動）、関節補間（関節空間移動）と直線補間（タスク空間移動）の３種類の補間方法が指定されている。

与えられた教示点と補間方法に対して、上記第１実施形態と同様に、図３の補間関数計算処理部３６１により、図８（ｂ）に示すように、区間ごとに区間補間関数が計算される。つまり、円弧補間に対応した区間補間関数ｈ_１（ｓ，ｐ_ｉ）関節補間に対応した区間補間関数ｈ_２（ｓ，ｐ_ｉ）と、直線補間に対応した区間補間関数ｈ_３（ｓ，ｐ_ｉ）が算出される。

区間補間関数は、円弧補間の区間、関節補間の区間及び直線補間の区間の全てを関節空間に統一して求めることが重要である。なお、区間補間関数をタスク空間に統一して表現することも可能である。しかし、その場合はサーボ制御装置２３０へ位置指令を送信する前に、逆運動学演算を解いて各軸角度へ変換する必要がある。つまり、計算コストの観点から、区間補間関数は関節空間で表現するのがよい。

第３実施形態では、３つに分かれている区間補間関数を図８（ｃ）のように接続を行う。図８（ｃ）の点線が各区間補間関数を接続した補間関数を模式的に示している。具体的には式（１）において区間補間関数番号ｋに対応する区間補間関数ｈ_ｋ（ｓ，ｐ_ｉ）を設定し、切り替える。つまり、補間教示点ｑ（ｓ）の算出に必要な補間関数を区間補間関数番号ｋによって切り替えるという処理を行う。つまり、円弧補間で指定されている区間は区間補間関数ｈ_１（ｓ，ｐ_ｉ）を使用し、関節補間で指定されている区間はｈ_２（ｓ，ｐ_ｉ）を、直線補間で指定されている区間はｈ_３（ｓ，ｐ_ｉ）を用いるという切り替えを行う。

以降の処理では、上記第１実施形態と同様、図８（ｄ）及び図８（ｅ）に示すように、通過速度及び位置指令が計算される。

以上説明したように、第３実施形態によれば、上記第１実施形態の効果に加えて、教示点に付随する補間方法が、円弧補間から関節補間に切り替わり、更に直線補間に切り替わる場合にも、ロボットアーム２００の動作時間を短縮することができる。

更に、複数の区間補間関数を個別に最適化する場合に比べ、複数の区間補間関数を１つの補間関数に結合した後に一度に全体を最適化することにより、ロボットアーム２００の動作時間を短縮することができる。

［第４実施形態］
次に、本発明の第４実施形態に係るロボット制御装置について説明する。図９は、本発明の第４実施形態に係るロボット制御装置の微分係数計算処理部の動作を説明するための模式図である。なお、第４実施形態では、微分係数計算処理部３６２（図３）の計算処理のみが上記第１〜第３実施形態と異なる。したがって、第４実施形態では、微分係数計算処理部３６２の計算処理について説明する。

上記第１〜第３実施形態では、補間関数が関節空間移動（Motion in configuration space）の場合を例に説明した。第４実施形態では補間関数がタスク空間移動（Motion in task space）の場合を説明する。

タスク空間移動の場合、補間関数はタスク空間で表現されている教示点を補間した後、ロボットアーム２００の逆機構学（inverse kinematics）計算を行って関節空間に変換し、補間教示点を求める。この時、複雑な逆機構学計算を行うため、解析的に微分係数を計算することが困難である。

第４実施形態では、上記第１〜第３実施形態と同様に、演算部としてのＣＰＵ３０１（図２）は、プログラム３３０に基づき、微分係数計算処理部３６２として機能する。

そして、第４実施形態では、微分係数計算処理部３６２が、補間関数が解析的に微分困難な場合に対応するため数値微分する。ただし、第４実施形態では、粗く補間することによって補間教示点の点数を減らしているので、単純に補間教示点間の値で数値微分すると、補間誤差が大きくなる。そこで、補間分割したそれぞれの位置において次のように微分係数を求める。

微分係数計算処理部３６２は、微分係数計算処理として、各補間教示点ｑ（１），ｑ（２）…における各ベクトル成分に対して変数ｓを各補間教示点の間隔よりも小さい微小値ｅで変化させたときの各ベクトル成分を補間関数に基づいて計算する。そして、微分係数計算処理部３６２は、該計算の結果を用いて、各微分係数を計算する。具体的には、微分係数計算処理部３６２は、各補間教示点ｑ（１），ｑ（２）…におけるベクトル成分の１階微分値、２階微分値、３階微分値を計算する。

詳述すると、まず、微分係数計算処理部３６２は、ｓ＝１，２…における補間関数の値、及び変数ｓを微小値ｅずらした位置での補間関数の値を、それぞれのｓの値について４点計算する。

次に、微分係数計算処理部３６２は、次式（１５）で１階微分値、２階微分値、３階微分値を計算する。

上式（１５）は一つの例であり、数値微分の公式はこの他にも多数知られている。この方法によれば、解析的に微分することが困難な補間関数であっても、１階微分値、２階微分値、３階微分値を精度よく計算でき、最短時間制御を実施できる。

［第５実施形態］
次に、本発明の第５実施形態に係るロボット装置について説明する。図１０は、第５実施形態に係るロボット装置の概略構成及び各ロボットアームの教示点を示す説明図である。

第５実施形態では、ロボット装置１００Ｂは、複数の多関節ロボットとして、２つのロボットアーム２００ａ，２００ｂと、これらロボットアーム２００ａ，２００ｂを制御するロボット制御装置３００Ｂと、を備えている。なお、第５実施形態では、演算部としてのＣＰＵの計算処理において、上記第１、第２実施形態とは補間関数を求める補間関数計算処理が異なるものであるが、他の計算処理は、上記第１〜第４実施形態と同様である。

したがって、第５実施形態では、ロボット制御装置３００Ｂは、２つのロボットアーム２００ａ，２００ｂを制御対象としているが、それぞれのロボットアームにおいて、補間関数計算処理以外は、上記第１〜第４実施形態と同様の処理を行うものである。

人間の組立では、両方の腕や手など、複数要素が同期して動く。同じようにロボットアームのロボット制御装置においても、複数のロボットアームの同期動作を実現できなければならない。

第５実施形態では、上記第１〜第４実施形態と同様に、演算部としてのＣＰＵ３０１（図２）は、プログラム３３０に基づき、補間関数計算処理部３６１として機能する。

そして、第５実施形態では、補間関数計算処理部３６１は、補間関数計算処理として、各教示点に、時刻に対応した教示点番号を付与し、該教示点番号を用いたスプライン補間により補間関数を求める。

具体的に説明すると、補間関数計算処理部３６１は、それぞれの教示点列の教示点に単調増加する教示点番号を設定し、その教示点番号を使った不等間隔スプラインによって教示点列の教示点間を補間する補間関数を求める。

例えば、図１０（ｂ）に示すように、第１のロボットアーム２００ａに対しては、教示点が４つ（^１ｐ_１，^１ｐ_２，^１ｐ_３，^１ｐ_４）、第１のロボットアーム２００ａに対しては、教示点が３つ（^２ｐ_１，^２ｐ_３，^２ｐ_４）あるものとする。これらの教示点で、後半の２つは両方のロボットアーム２００ａ，２００ｂを協調させて作業するため同時刻に通過させたいとする。

それぞれの教示点列において単調増加する教示点番号は１番からスタートするが、第２のロボットアーム２００ｂについては２番が抜けている。２つの教示点列において、同じ教示点番号のところで同期した動きを実現するのが目的である。つまり、２番を除いた１番、３番、４番の教示点をそれぞれのロボットアーム２００ａ，２００ｂが同時に通過することが目的である。

これは、補間関数を不等間隔スプライン関数にすることによって実現できる。不等間隔スプライン関数は例えば、Ｂスプラインなどが知られている。

とする。両者の要素を合体し、改めて補間関数として次のように表記する。

これ以降の処理は、上記第１、第２実施形態と同様である。

第５実施形態では、上記第１及び第２実施形態の効果に加えて、教示番号が同じところ、上式では同じｓの値のところでロボットアーム２００ａ，２００ｂを同期させることができる。つまり複数のロボットアームの同期動作に対応した最短時間制御を実現できる。

［第６実施形態］
次に、本発明の第６実施形態のロボット制御装置について説明する。図１１は、本発明の第６実施形態に係るロボット制御装置における制約条件違反率計算処理部の処理動作を説明するための図である。

第６実施形態のロボット制御装置では、予め定めておいた制約値（許容値）が、補間教示点ごとに異なる値に設定されている。あらかじめ異なる制約値を教示点ごとに与え、その値に従って式（８）を用いて制約条件違反率を計算する。その他の計算処理は上記第１〜第５実施形態と同様であるので、説明を省略する。

上記第１〜第５実施形態の制約条件違反率計算処理部３６８では、制約条件違反率として、推定値と予め定めた許容値との比を計算していた。例えば第ｊ軸の関節の速度の場合、上記第１〜第５実施形態では、関節の推定速度と定数である許容速度Ｖｊとの比、

を計算していた。

第６実施形態では、許容速度が変数の値に応じて変化する値に設定され、推定速度と教示点ごとに異なる許容速度との比、

が計算される。

図１１示す横軸は補間教示点の番号を示す変数ｓであり、変数ｓは補間するので整数に限らず、連続した数字である。この図１１は、通過速度の最適化計算が完了した後を図示している。従って関節の推定速度は許容速度を超えていない。

第６実施形態では、教示点ごとに制約条件を設定できるので、上記第１〜第５実施形態よりもさらに制約条件の自由度が向上する。

更に、第６実施形態では、図１１に示すように、許容速度が、変数ｓの値が増加するに連れて小さくなるように設定されている。これにより、ロボットアームが終点に近づくに連れて関節の速度が低下するので、ロボットアームを用いた組立作業を安定して行うことができる。

なお、第６実施形態では、関節の速度の制約について説明したが、関節の加速度や加加速度、トルク、手先速度、手先回転速度など他の制約条件についても同じように、教示点ごとに許容値を設定できる。

以上説明したように、第６実施形態によれば、上記第１〜第５実施形態の効果に加えて、教示点ごとに異なる許容値を設定してロボットアームの最短時間制御を実現できる。

［第７実施形態］
次に、本発明の第７実施形態に係るロボット制御装置について説明する。第７実施形態では、制約条件には、上記第１〜第６実施形態における条件の他、更に、推定モータトルクと予め定めた許容モータトルクとの比が所定制約範囲内（±１の範囲内）となる条件が含まれている。

ロボットアーム２００の関節Ｊ１〜Ｊ６と電動モータ２１１〜２１６は減速機２２１〜２２６を介して接続されている。この減速機２２１〜２２６と電動モータ２１１〜２１６との計算モデルから各電動モータ２１１〜２１６における推定モータトルクが計算できる。

図１２は、第７実施形態に係る電動モータのモデル及びＴＮ曲線の説明図である。図１２（ａ）に電動モータ２１１〜２１６のモデルを示す。ここで次の記号を使った。
Ｎ_ｊ 第ｊ軸の減速機の減速比
τ_ｊ 第ｊ軸モータの推定モータトルク
Ｉ_ｊ 第ｊ軸モータの慣性モーメント
ｃ_ｊ 第ｊ軸の粘性要素
ｄ_ｊ 第ｊ軸の摩擦要素

このモデルを具体的に式で表現する。制約条件を計算するときに使った関節の推定速度、推定加速度および関節にかかる推定トルクを用いる。電動モータ２１１〜２１６の推定モータトルクは、減速比Ｎ_ｊを考慮して次のように書ける。

右辺第１項は式６で計算した関節にかかるトルクである。第２項はモータの慣性力、加速度に比例した成分である。第３項はモータの粘性力、速度に比例した成分である。第４項はモータの摩擦力、速度の符号に応じて、符号が変わる成分である。

推定値計算処理部３６５は、更に、各補間教示点ｑ（ｓ）における電動モータ２１１〜２１６の推定モータトルクを、減速機２２１〜２２６の減速比、各通過速度及び各微分係数に基づいて計算する。なおこの場合、推定トルクｕ_ｊ（ｓ）は、式（４）、式（５）及び式（６）から求めることができるので、各補間教示点、各通過速度（通過速度、通過速度の１階及び２階微分値）及び各微分係数（補間関数の１階、２階微分値）に基づいて計算されることとなる。

この式（１７）から明らかなように、左辺のモータのトルクは、第１項の関節にかかるトルクよりも大きい。つまり減速機の慣性や粘性などの影響をうけて、モータには、より大きなトルクが必要である。

このようにして計算された推定モータトルクは、許容モータトルクを超えてはならない。許容モータトルクは、一般に、横軸にモータの速度、縦軸にモータのトルクをとったＴＮ曲線と呼ばれるグラフで表現される。この推定モータトルクと許容モータトルクとの比についても制約条件違反率の計算に加えることによってモータトルクも考慮した最短時間制御が実現できる。

ＴＮ曲線の例を図１２（ｂ）に示す。このように許容モータトルクは一般に速度の関数である。図３で説明した制約条件違反率計算処理部３６８に次の制約条件違反率の計算を追加する。

６軸多関節ロボットの場合ｊ＝１〜６種類ある。

図１２（ｂ）では、推定モータトルクが、許容範囲にすべて入っている状況を示している。つまり上記の制約条件違反率の大きさが１以下となっている。こうして制約条件違反率を計算する以外の部分は、上記第１〜第６実施形態と同様であり説明を省略する。

以上説明したように、第７実施形態によれば、上記第１〜第６実施形態の効果に加え、モータトルクについても制約条件を設定できるので、上記第１〜第６実施形態よりもさらに制約条件の自由度を向上できる。そして、許容モータトルクの制約条件を満たしたロボットアームの最短時間制御を実現できる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。

上記第１〜第７実施形態では、全ての種類の制約条件を考慮したが、これらの制約条件を常に考慮する必要はない。例えば推定手先速度や推定手先回転速度に関係なく、動作速度を優先した動作は、推定手先速度や推定手先回転速度の制約条件をα_ｎ（ｓ）の種類からはずすだけで、実現できる。このようにどの制約の種類を使うかは予め定めておけばよい。その際、微分係数計算処理部３６２は、必要な微分係数のみ計算するようにすればよい。また、通過速度微分係数計算処理部３６４、推定値計算処理部３６５においても、必要な微分値、推定値のみ計算するようにすればよい。特に、推定速度のみ計算する場合は、通過速度微分係数計算処理部３６４は省略可能である。

また、上記第１〜第７実施形態では、ロボット制御装置の制御対象である多関節ロボットがロボットアームの場合について説明したが、多関節ロボットがロボットアームの先端に設けられるロボットハンドであっても適用可能である。

また、補間方法が複数に分かれて指定された実施形態として、上記第１〜３実施形態に、直線補間、関節補間、円弧補間の場合を説明したが、補間方法はこれらに限定されるものではない。

また、補間方法が複数に分かれて指定された実施形態として、上記第１〜３実施形態に、切り替えが１〜２度という場合について説明したが、切り替え回数はメモリの許す限り対応することが可能である。

また、以上述べた実施形態の各処理動作は具体的にはロボット制御装置の演算部としてのＣＰＵ３０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体をロボット制御装置３００に供給し、ロボット制御装置３００のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。

また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０が格納される場合について説明したが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図２に示すＨＤＤ３０４、外部記憶装置３２２、記録ディスク３３１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。

また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。

また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。

１００…ロボット装置、１０１…ＣＰＵ（演算部）、２００…ロボットアーム（多関節ロボット）、２０１〜２０６…アクチュエータ、２１１〜２１６…電動モータ、２２１〜２２６…減速機、２３０…サーボ制御装置（駆動制御部）、３００…ロボット制御装置、３３０…プログラム、３３１…記録ディスク（記録媒体）

Claims (15)

1. 多関節ロボットが、指定された複数の教示点を辿って動作するよう、前記多関節ロボットの各関節を駆動する各アクチュエータの動作を位置指令に基づいて制御する駆動制御部に、位置指令を出力するロボット制御装置において、
   前記複数の教示点における始点から終点までの間を複数の区間に区切り、前記多関節ロボットの位置及び姿勢を規定するタスク空間、及び前記多関節ロボットの各関節角度を規定する関節空間のうち、いずれか一方に統一して、前記各区間にそれぞれ指定された補間方法で区間補間関数をそれぞれ求める区間計算処理と、
   前記各区間補間関数を結合して全区間に跨る、補間教示点の通過順番を示す変数を用いた補間関数を求める補間関数計算処理と、
   前記補間関数から求められる各補間教示点を前記変数で微分した微分係数をそれぞれ計算する微分係数計算処理と、
   前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定処理と、
   前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算処理と、
   前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記教示点の始点から終点に到達する時間を最小化する最短時間制御処理と、
   前記最短時間制御処理で得られた前記各通過速度から通過速度関数を求める通過速度関数計算処理と、
   前記通過速度関数から前記位置指令を生成する位置指令生成処理と、を実行する演算部を備えたことを特徴とするロボット制御装置。
2. 前記演算部は、前記微分係数計算処理として、前記各補間教示点の各ベクトル成分に対して前記変数を前記各補間教示点の間隔よりも小さい微小値で変化させたときの各ベクトル成分を前記補間関数に基づいて計算し、該計算の結果を用いて、前記各微分係数を計算することを特徴とする請求項１に記載のロボット制御装置。
3. 前記演算部は、前記補間関数計算処理として、前記各教示点に、時刻に対応した教示点番号を付与し、該教示点番号を用いたスプライン補間により前記補間関数を求めることを特徴とする請求項１又は２に記載のロボット制御装置。
4. 前記許容速度が、前記変数の値に応じて変化する値に設定されていることを特徴とする請求項１乃至３のいずれか１項に記載のロボット制御装置。
5. 前記許容速度が、前記変数の値が増加するに連れて小さくなるように設定されていることを特徴とする請求項４に記載のロボット制御装置。
6. 前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節について前記推定速度を時間で１階微分した推定加速度を、前記各通過速度及び前記各微分係数に基づいて計算し、
   前記制約条件には、更に、前記推定加速度と予め定めた許容加速度との比が前記所定制約範囲内となる条件が含まれる、
   ことを特徴とする請求項１乃至５のいずれか１項に記載のロボット制御装置。
7. 前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節について前記推定速度を時間で２階微分した推定加加速度を、前記各通過速度及び前記各微分係数に基づいて計算し、
   前記制約条件には、更に、前記推定加加速度と予め定めた許容加加速度との比が前記所定制約範囲内となる条件が含まれる、
   ことを特徴とする請求項１乃至６のいずれか１項に記載のロボット制御装置。
8. 前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記各関節の推定トルクを、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
   前記制約条件には、更に、前記推定トルクと予め定めた許容トルクとの比が前記所定制約範囲内となる条件が含まれる、
   ことを特徴とする請求項１乃至７のいずれか１項に記載のロボット制御装置。
9. 前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記多関節ロボットの先端の推定先端速度を、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
   前記制約条件には、更に、前記推定先端速度と予め定めた許容先端速度との比が前記所定制約範囲内となる条件が含まれる、
   ことを特徴とする請求項１乃至８のいずれか１項に記載のロボット制御装置。
10. 前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記多関節ロボットの先端の推定回転速度を、前記各補間教示点、前記各通過速度及び前記各微分係数に基づいて計算し、
    前記制約条件には、更に、前記推定回転速度と予め定めた許容回転速度との比が前記所定制約範囲内となる条件が含まれる、
    ことを特徴とする請求項１乃至９のいずれか１項に記載のロボット制御装置。
11. 前記アクチュエータが、電動モータ及び減速機を有しており、
    前記演算部は、前記推定値計算処理として、更に、前記各補間教示点における前記電動モータの推定モータトルクを、前記減速機の減速比、前記各通過速度及び前記各微分係数に基づいて計算し、
    前記制約条件には、更に、前記推定モータトルクと予め定めた許容モータトルクとの比が前記所定制約範囲内となる条件が含まれる、
    ことを特徴とする請求項１乃至１０のいずれか１項に記載のロボット制御装置。
12. 多関節ロボットと、
    前記多関節ロボットを制御する請求項１乃至１１のいずれか１項に記載のロボット制御装置と、を備えたことを特徴とするロボット装置。
13. 多関節ロボットが、指定された複数の教示点を辿って動作するよう、前記多関節ロボットの各関節を駆動する各アクチュエータの動作を位置指令に基づいて制御する駆動制御部に、出力する位置指令を生成する演算部を有するロボット制御装置を用いたロボット制御方法において、
    前記演算部が、前記複数の教示点における始点から終点までの間を複数の区間に区切り、前記多関節ロボットの位置及び姿勢を規定するタスク空間、及び前記多関節ロボットの各関節角度を規定する関節空間のうち、いずれか一方に統一して、前記各区間にそれぞれ指定された補間方法で区間補間関数をそれぞれ求める区間計算工程と、
    前記演算部が、前記各区間補間関数を結合して全区間に跨る、補間教示点の通過順番を示す変数を用いた補間関数を求める補間関数計算工程と、
    前記演算部が、前記補間関数から求められる各補間教示点を前記変数で微分した微分係数をそれぞれ計算する微分係数計算工程と、
    前記演算部が、前記各補間教示点において前記変数を時間微分した微分値を示す通過速度をそれぞれ仮設定する仮設定工程と、
    前記演算部が、前記各補間教示点における前記各関節の推定速度を、前記各通過速度及び前記各微分係数に基づいて計算する推定値計算工程と、
    前記演算部が、前記各関節の推定速度と予め定めた許容速度との比が所定制約範囲内となる条件を含む制約条件の下、前記各通過速度を変更して、前記教示点の始点から終点に到達する時間を最小化する最短時間制御工程と、
    前記演算部が、前記最短時間制御工程で得られた前記各通過速度から通過速度関数を求める通過速度関数計算工程と、
    前記演算部が、前記通過速度関数から、前記位置指令を生成する位置指令生成工程と、を備えたことを特徴とするロボット制御方法。
14. コンピュータに請求項１３に記載のロボット制御方法の各工程を実行させるためのプログラム。
15. 請求項１４に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images