ロボットの制御装置及び方法
【課題】ロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合に、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる制御装置及び制御方法を提供する。
【解決手段】本制御装置及び制御方法は、少なくとも１つの関節を有するロボットにおいて、各関節につき、関節位置指令に対して、位置フィードバック制御及び速度フィードバック制御を行い、該関節に設定された関節座標系において、所定の制限開始位置と該関節の現在位置との関係及び所定の制限開始速度と該関節の現在速度との関係に基づいて制限トルクを計算し、計算された制限トルクを速度フィードバック制御の出力に加算して得られるトルクにより該関節の動作を制御する。
【解決手段】本制御装置及び制御方法は、少なくとも１つの関節を有するロボットにおいて、各関節につき、関節位置指令に対して、位置フィードバック制御及び速度フィードバック制御を行い、該関節に設定された関節座標系において、所定の制限開始位置と該関節の現在位置との関係及び所定の制限開始速度と該関節の現在速度との関係に基づいて制限トルクを計算し、計算された制限トルクを速度フィードバック制御の出力に加算して得られるトルクにより該関節の動作を制御する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施例は、ロボットの制御装置及び方法に関し、例えばロボットの柔軟制御における可動域制限装置及び方法に関する。
【背景技術】
【０００２】
産業用ロボットの柔軟制御は、力センサなどを使用することなくロボットアームを直接操作して位置教示を行う場合や、周辺機器がロボットに外力を及ぼす状況下でのハンドリング作業などにおいて使用される。この柔軟制御を用いて作業する場合、操作者や周辺機器からの外力によってロボットがその可動域を超えて無制限に動作されることにより、ロボットのメカリミットの破損やロボットと別の周辺機器との干渉が発生する危険性があるため、何らかの方法で可動領域を制限する必要がある。
【０００３】
従来の可動域制限方法で一般に知られているものとして、ロボットコントローラに実装されたソフトウェア上でロボットアーム各軸のモータにつき動作制限位置を予め設定しておき、いずれかの軸のモータが当該動作制限位置に達した場合にアラームを発生し、ロボットを緊急停止させる手法がある。
【０００４】
また、前記手法と同様に、ソフトウェア上でロボットアームの各軸のモータについて動作制限位置を予め設定しておき、各軸の位置（関節角度、つまりモータ回転角度である）が当該動作制限位置に近づくにつれて漸増する力を所定の方法で計算し、計算された力を反力として加えることにより、各軸の動きが可動域を超えることを回避する手法がある（例えば、特許文献１参照）。
【０００５】
さらに、機構的な対処法として、ロボットの各軸の位置が機械的動作限界へ近づくにつれて大きな抵抗値を示すようになる抵抗部材をロボットの関節部に設けるものもある（例えば、特許文献２参照）。
【０００６】
図５は、特許文献１に記載の手法をマスタースレーブマニピュレータに適用し動作限界の反力提示を行う制御装置を示すブロック図である。スレーブアーム側の関節角度Ｐｓが所定の作動限界近傍に達すると、その関節角度Ｐｓに応じて計算される反力ＴＬをマスターアームの反力に足しこむように構成されている。図６はスレーブアームの関節角度Ｐｓと外力に加える反力ＴＬとの関係について予め設定される関係を示す。図６(ａ)は関節角度Ｐｓに関係なくＴＬ＝０とするものである。図６（ｂ）は、反力ＴＬが関節角度Ｐｓと所定の作動限界近傍の制限値ＰＬＬとの差に比例して増加するように与えられる例を示す。図６（ｃ）は、ＰｓとＰＬＬとの差の２乗に比例するように反力ＴＬを決定する例を示す。
【０００７】
このように、従来のロボットアームの可動域制限方法では、有力な手法として、ロボットアームの各軸について所定の制限位置（関節角度）と現在位置との差から反力を計算し、当該反力を加えることによって各軸の動作が可動域を超えることが回避されていた。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平１−１８３３７９号公報
【特許文献２】実公昭６２−０３１２６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
各軸について動作制限位置を設けロボットアーム各軸の現在位置を常時監視する従来の方法は、いずれかの軸が制限位置に到達した時にロボットアームを緊急停止させるという手順をとっているので、復旧に時間や手間がかかるという問題がある。
【００１０】
また、各軸に関して予め設定された関節制限位置と現在位置との差から計算される反力を加える方法においては、反力がロボットの各軸の位置のみに基づいて計算されるため、柔軟制御中に急激に大きな外乱トルクが印加されていずれかの軸の位置が制限領域に到達した場合や高速で制限領域に到達した場合には、逆向きのトルクが過大となり、減速機などに大きな負担がかかるという問題があった。
【００１１】
さらに、機構的な対処法には、各軸が繰り返し制限領域に近づくような場合に制限機構が劣化し易いという問題があった。
【００１２】
したがって、従来技術によると、ロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合には、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができなかった。
【課題を解決するための手段】
【００１３】
上記問題を解決するため、本願は、次のような発明を提供する。
【００１４】
なお、本願出願時の明細書の記載を基礎として、特許請求の範囲の記載が補正され得ることは勿論であるが、その際に、本願発明に関して記載される以下の概要の内容を訂正することは予定しない。以下の概要に記載される各「発明」は、本願明細書における開示の要点としての意味を持つものである。
【００１５】
本発明の１つの側面において、本発明の制御装置は、少なくとも１つの関節を有するロボットの柔軟制御に使用されるものであり、各関節につき、関節位置指令に対して、該関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算する制限トルク計算手段であって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限トルクが大きくなる、制限トルク計算手段とを備える。そして、当該制御装置は、速度フィードバック制御手段の出力に計算された制限トルクを加算して得られるトルクにより該関節の動作を制御する。
【００１６】
制限トルクは、制限開始位置と現在位置との差に基づいて計算される第１の制限トルクと、制限開始速度と現在速度との差に基づいて計算される第２の制限トルクとの和として計算されてもよい。当該制限トルクは、本明細書において後述する式(１)から式(７)に基づいて計算してもよい。
【００１７】
このように構成することによって、関節座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００１８】
また、本発明の別の側面において、本発明の制御装置は、少なくとも１つの関節を有するロボットの柔軟制御に使用されるものであり、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定する順運動学計算部と、手先位置指令に対して、各関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、作業座標系において、手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と手先の現在位置との関係、及び手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と手先の現在速度との関係に基づいて、制限力を計算する制限力計算手段であって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限力が大きくなる、制限力計算手段と、計算された制限力を速度フィードバック制御手段の出力に加算して得られる力から各関節に要求されるトルクを計算する逆計算手段とを備える。そして、当該制御装置は、計算されたトルクにより各関節の動作を制御する。
【００１９】
制限力は、制限開始位置と現在位置との差に基づいて計算される第１の制限力と制限開始速度と現在速度との差に基づいて計算される第２の制限力との和として計算されてもよい。当該制限力は、本明細書において後述する式(８)から式(１４)に基づいて計算してもよい。
【００２０】
このように構成することによって、作業座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２１】
また、本発明の別の側面において、本発明の制御方法は、少なくとも１つの関節を有するロボットの柔軟制御に使用され、少なくとも１つの関節につき、関節位置指令に対して、該関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算するステップであって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限トルクが大きくなる、制限トルクを計算するステップと、計算された制限トルクを速度フィードバック制御の出力に加算して得られるトルクにより該関節の動作を制御するステップとを備える。
【００２２】
このように構成することによって、関節座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２３】
また、本発明の別の側面において、本発明の制御方法は、少なくとも１つの関節を有するロボットの柔軟制御に使用され、順運動学計算により、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定するステップと、手先位置指令に対して、各関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、作業座標系において、手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と手先の現在位置との関係、及び手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と手先の現在速度との関係に基づいて、制限力を計算するステップであって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限力が大きくなる、制限力を計算するステップと、逆計算により、計算された制限力を速度フィードバック制御の出力に加算して得られる力から各関節に要求されるトルクを計算するステップと、計算されたトルクにより各関節の動作を制御するステップとを備える。
【００２４】
このように構成することによって、作業座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２５】
審査中の手続補正によって、及び、特許後の訂正審判又は訂正請求において、法的な制限の範囲内で、本発明は、種々の他の態様に訂正され得る。
【００２６】
なお、特許後の訂正審判又は訂正請求における「実質上特許請求の範囲を変更」の判断は、特許時の請求項に新たな構成要素が追加されたか否か（すなわち、いわゆる外的付加がなされたか否か）、又は、特許時の請求項の１つ又はそれより多いいずれかの構成要素をさらに限定するものか（すなわち、いわゆる内的付加がなされたか）によって判断されるべきでなく、訂正の前後の請求項に係る発明の効果が類似するか否かの観点からなされるべきである。
【発明の効果】
【００２７】
本発明の実施例によると、関節座標系上でロボットの柔軟制御を実行した際に、ロボットが可動域限界に到達したときにロボットを緊急停止することなく機械的可動域を保護し、周辺機器への干渉回避を行うことができる。
【００２８】
また、作業座標系上でロボットの柔軟制御を実行した際に、ロボットが可動域限界に到達したときにロボットを緊急停止することなく機械的可動域を保護し、周辺機器への干渉回避を行うことができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の一実施例によるロボットシステムの構成図である。
【図２】本発明の一実施例の装置又は方法の処理手順を示すフローチャートである。
【図３】本発明の一実施例による可動域制限装置又は方法を関節座標系での柔軟制御に適用した場合の説明図である。
【図４】本発明の一実施例による可動域制限装置又は方法を作業座標系での柔軟制御に適用した場合の説明図である。
【図５】従来の方法を適用したマスタースレーブマニピュレータの制御ブロック図である。
【図６】図５に示す従来の方法における反力ＴＬと関節角度Ｐｓとの関係を説明するグラフである。
【発明を実施するための形態】
【００３０】
以下、本発明の装置及び方法の具体的実施例について、図に基づいて説明する。
【００３１】
ここでは、本発明の一実施例として、本発明の装置又は方法をロボットの直接教示（ダイレクトティーチング）に適用した場合について説明する。ここで、教示（ティーチング）とは、ロボットに作業を行わせるためにその作業を実現する動作を何らかの方法で教え込むことを意味する。直接教示とは、教示装置を介することなく、操作者がある作業のための所望の動作手順に沿ってロボットに力を加えて手動でロボットを動かすことにより、当該動作をロボットに教え込む教示手法である。
【００３２】
図１は、本発明の可動域制限装置及び方法が実施されるロボットシステムの概略的な構成図を示す。図１において、ロボットシステムは、６軸の垂直多関節型ロボット１、本発明の可動域制限装置の実施例であり、本発明の可動域制限方法の実施例を実現するコントローラ２、及び教示の際に各種設定を行うための教示装置３を備えている。図１においては、ロボットアームの直接教示を行う操作者４も図示されている。図１においてロボット１は６軸の垂直多関節型を例にしているが、これに限定されない。本発明は、少なくとも１つの関節を有する様々なロボットに対して適用することが可能である。教示装置３は、コントローラ２と有線又は無線で接続されており、表示画面及び複数のボタンを備え、操作者４により把持されて操作される。教示装置３は、ロボット１に所定の作業を行うために必要となる動作を教示したり、ロボット１の状態を把握したり、教示した作業の開始、一時停止、非常停止などを指令したりするのに使用される。教示装置３は、教示ペンダント、ティーチングボックス、ティーチングペンダント、プログラミングペンダント等とも呼称される。教示装置３を用いた操作の一例として、ジョグ運転と呼ばれる操作を行ってもよい。ジョグ運転とは、操作者４が教示装置３上の各軸動作ボタンを押している間だけロボット１の所定の軸が動作し、放すと動作が停止するという操作である。操作者４は、ジョグ運転により、ロボット１を所望の位置や姿勢へと動かし、教示装置３を介して教示操作を行うことにより、そのときのロボット１の位置及び姿勢をコントローラ２に記憶させることができる。
【００３３】
ロボット１の柔軟制御を行う場合、通常の状態では、ロボットアームは外力に倣って動作する。柔軟制御とは、ロボットアームに作用する重力や摩擦力等のみを補償するトルク指令を与え、ロボットアームを外力に倣って動作させる制御方法である。
【００３４】
後述するように、本発明によれば、操作者４が教示装置３を介して可動域制限に関するパラメータをコントローラ２に登録しておくと、ロボット１の各軸（各関節）の動作が可動域限界に近づいた場合にコントローラ２が適切な反力を計算して各軸に印加する。これにより、各軸の位置（回転角度）が可動域限界に達することを回避することができる。
【００３５】
図２は、コントローラ２において実行される可動域制限の処理手順を示すフローチャートである。以下、図２を用いるとともに必要に応じて図３及び図４を参照しながら処理手順を順を追って説明する。
【００３６】
はじめに、Ｓ１１において、柔軟制御を実行する座標系について関節座標系か作業座標系かを選択する。ここで、関節座標系とは、ロボットの各軸に固定された座標系であり、ロボットを動作させる際の基準として使用される座標系を意味する。また、作業座標系とは、ロボットの手先部に固定された座標系であり、直交座標系とも呼称される。ロボットの柔軟制御は、関節座標系に基づいて行われる制御と作業座標系に基づいて行われる制御とに大別されるため、Ｓ１１においていずれの座標系に基づく柔軟制御を実行するかを選択する。当該選択は、制御対象のロボット１に関してコントローラ２が予め記憶している情報に基づいてコントローラ２が行ってもよいし、操作者４が教示装置３を介してコントローラ２へ選択の指示を送ることにより行われてもよい。その他、当業者に周知の様々な手法により、コントローラ２はＳ１１を実行することができる。
【００３７】
ここで、Ｓ１１において柔軟制御の座標系として関節座標系が指定された場合において実施される本発明の一実施例の可動域制限装置及び可動域制限方法の構成及び原理に関して、図３を用いて説明する。図３（ａ）は、関節座標系に基づきコントローラ２において可動域制限を実行する可動域制限付き位置制御部２０１のブロック図である。
【００３８】
図３（ａ）においては、２０１ａ乃至２０１ｃの３つのブロック図が示されており、添え字のアルファベットはロボットの各軸に対応する。可動域制限はロボット１の各軸（各関節）ごとに行う必要がある。すなわち、６軸を有するロボットであれば６つの軸に対してそれぞれ同様の制御を行う必要がある。
【００３９】
以下、軸ａに対応する可動域制限付き位置制御部２０１ａを例にとり動作を説明する。図３（ａ）に示される関節位置指令は、ロボット１の手先を移動させるにあたって各軸に要求される関節位置（回転角度）を示す指令である。操作者が教示装置３を使用してロボット１にある作業に関する動作を教え込む教示の際には、関節位置指令は操作者による教示装置３への入力に基づいて、コントローラ２へ送られる。既に教示された動作をロボット１が実行する再生動作の場合には、関節位置指令はコントローラ２から得られる。また、直接教示の場合には、操作者がロボットを直接操作し、所望する位置に導く。この際、教示装置３は直接教示に関する数値設定や設定切換えに使用される。このような直接教示の際には、操作者がロボットに印加した力に基づいて各関節に位置指令が与えられることになる。一般的に柔軟制御は周辺機器がロボットに外力を及ぼす状況下でのハンドリング作業などにおいて使用され、外力を吸収するという受動的な使用形態をとる。本実施例では直接教示の際に柔軟制御を適用しているが、ハンドリング作業などの際に本発明を適用することももちろん可能である。再生（playback）とは、ロボットが、記憶した情報によって指示された作業・動作を実行することを意味する。
【００４０】
可動域制限付き位置制御部２０１ａにおいて、軸ａの位置は、軸ａに内蔵された駆動モータ２０６ａのエンコーダ（図示せず）によって検出される。エンコーダは、モータの位置（回転角度）を測定するためのセンサである。エンコーダの出力を時間微分するとモータの回転（角）速度が得られる。ロボット制御において、エンコーダは、通常、各軸を駆動するモータに設けられており、エンコーダにより測定された各軸の位置（回転角度）とロボットの各リンク（軸と軸とを繋ぐ部分）の既知の長さとからロボットの手先の位置及び姿勢を求めることができる。このようにしてロボットの手先の位置及び姿勢を求めることを順運動学という。
【００４１】
エンコーダ（図示せず）によって検出された軸ａの位置は、加算部２０８ａ及び位置フィードバックゲイン要素Ｋｐ２０９ａからなる、位置フィードバック制御を行う関節座標系位置フィードバックループと、速度演算部２０７ａ、加算部２１０ａ及び速度フィードバックゲイン要素Ｋｖ２１１ａからなる、速度フィードバック制御を行う関節座標系速度フィードバックループとによって、関節位置指令にフィードバックされる。速度フィードバックゲイン要素Ｋｖ２１１ａの出力は対応するトルクの値へと変換されて、トルク制限部２０３ａへ入力されてもよい。Ｋｖの乗算により、制御的に速度からトルクへの単位変換が行われる。Ｋｐの出力及びＫｖの入力は関節速度指令となる。トルク制限部２０３ａは、ロボット１の軸ａのモータの発生トルクに対して予め与えられた制限（０％〜１００％）を与えるものであり、その制限値はロボット１の動作目的や動作環境等に応じて予め適切に定められる任意の値である。トルク制限部２０３ａを用いることにより、軸ａのモータのトルクを所定のトルク制限値内に留めることができる。
【００４２】
関節座標系で柔軟制御を実行する場合、上述のようにトルク制限部２０３においてロボットの各軸のモータの発生トルクが設定値以上にならないように制限を与えたり、位置フィードバックゲインＫｐや速度フィードバックゲインＫｖを可変にしたりすることが有用である。
【００４３】
重力補償トルク計算部２０４ａ及び摩擦補償トルク計算部２０５ａは、それぞれ、重量や重心位置などのロボット１に関するリンクパラメータに基づいて重力補償トルクを計算し、摩擦モデルから速度に応じた摩擦補償トルクを計算する。
【００４４】
図３(ｂ)は、ａ〜ｆの６軸を有するロボット１の概略図であり、各軸と、軸同士を接続するリンクとが示されている。ａ〜ｆの各軸は限界位置（限界回転角度）を有しており、これを超えて回転がなされると故障等につながる。
【００４５】
軸１０１ｂを例として説明する。図３（ｂ）に示されるθＰＴｒｍｂは、軸ｂの限界位置（限界回転角度）を示す。軸ｂがこのθＰＴｒｍｂまで回転することを回避するため、本発明では、限界位置θＰＴｒｍｂから一定量だけ戻した（すなわち、一定量だけ絶対値の小さな）位置（以下、「制限開始位置」という。）であるθＰＬｉｍｂを予め決定しておく。そして、軸ｂの位置が図３（ｂ）に示すように制限開始位置θＰＬｉｍｂに達するかもしくはこれを超えると、所定の計算手法に従って、図３（ａ）における関節座標系可動域制限トルク計算部２０２ｂ（図３（ａ）における軸ｂの可動域制限トルク計算部である。軸ｂに対応する可動域制限付き位置制御部２０１ｂに存在する）において反力ＴＬｎｋｂを計算し、これを加算して得たトルクを軸１０１ｂのモータに指示する。例えば直接教示（ダイレクトティーチング）を行っている操作者は、それまで柔軟に制御できていたロボット１の動作が反力を受けたことを認識することができるので、ロボット１が可動域限界に近づいていることを認識することができる。なお、図中のその他の軸に関しても反力を記しているが、太線、細線、点線となるに従って反力が弱くなることを意味している。
【００４６】
図３(ｃ)及び図３(ｄ)は可動域制限トルク計算部２０２において実行される計算を説明する図である。図３（ｃ）において、縦軸は計算対象の軸の位置（回転角度）に基づいて決定される反力（制限トルク）を表し、横軸は計算対象の軸の位置を表す。図３（ｄ）において、縦軸は計算対象の軸の速度（角速度）に基づいて決定される反力を表し、横軸は計算対象の軸の速度（角速度）を表す。反力を滑らかに提示するよう、図３（ｃ）及び図３（ｄ）に示すように、制限開始位置（θＰＬｉｍ、θＭＬｉｍ）及び制限開始速度（ωＰＬｉｍ、ωＭＬｉｍ）から立ち上がり、限界位置（θＰＴｒｍ、θＭＴｒｍ）及び限界速度（ωＰＴｒｍ、ωＭＴｒｍ）に漸近するような２次以上の曲線によって反力が表されるようにするのが好ましい。但し、反力と位置又は速度との関係は図３（ｃ）及び図３（ｄ）に示される関係に限定されない。反力は、軸の現在位置が制限開始位置を超えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて、大きくなるように規定されればよい。軸が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくともいずれかを超えた場合に限界位置又は限界速度に達することを回避するのに有用な様々な計算式を設定し得る。図３（ｃ）において、θＭＬｉｍ及びθＰＬｉｍは、それぞれ負方向及び正方向の制限開始位置であり、θＭＴｒｍ及びθＰＴｒｍは、それぞれ負方向及び正方向の限界位置である。図３（ｄ）に示される各値も、同様に、負方向及び正方向の制限開始速度並びに負方向及び正方向の限界速度をそれぞれ意味している。既に述べたように、制限開始位置及び制限開始速度は、それぞれ限界位置及び限界速度から所定の値だけ原点方向に近づいた値に設定される。
【００４７】
ここで再び図２に戻り、本発明の一実施例による関節座標系の場合の可動域制限の処理手順を説明する。
【００４８】
Ｓ２１において、関節座標系においてロボットの各軸に配置されたモータの発揮するトルクの制限値を、例えば教示装置３から設定する。ここで設定されたトルク制限は図３（ａ）に示すトルク制限部２０３において使用される。
【００４９】
次に、Ｓ３１において、関節座標系において柔軟制御を開始する。すなわち、ロボット１のアームが外力に倣って動作するような状態にロボット１を設定する。
【００５０】
さらに、Ｓ４１において、関節座標系の可動域制限に必要となる制限トルクＴＬｎｋ（図３参照）を、例えば以下に示す式(１)から式(７)により計算するために必要となる各種のパラメータを設定する。この計算は、図３（ａ）の例においては、関節座標系可動域制限トルク計算部２０２ａにおいて実行される。なお、制限トルクＴＬｎｋの計算方法は、以下に示すものに限定されるものではなく、既に述べたとおり、各軸（各関節）の動作が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくともいずれかを超えた場合に限界位置又は限界速度に達することを回避するのに有用な様々な計算手法を適用可能であることは当業者にとって明らかであろう。
【数１】
ここで、ＴＬｎｋ_θ及びＴＬｎｋ_ωは、それぞれ、各軸の位置（回転角度）に基づいて計算される制限トルク及び各軸の速度（角速度）に基づいて計算される制限トルクであり、例えば図３（ｃ）及び図３（ｄ）に示すような特性を有する。ｋＰθ及びｋＭθはそれぞれ正負方向の位置による制限トルク計算定数、ｋＰω及びｋＭωはそれぞれ正負方向の速度による制限トルク計算定数である。θ及びωは現在の位置（回転角度、関節角度）及び速度（角速度）、θＰＬｉｍ及びθＭＬｉｍはそれぞれ正負方向の制限開始位置、θＰＴｒｍ及びθＭＴｒｍはそれぞれ正負方向の限界位置である。ωＰＬｉｍ及びωＭＬｉｍはそれぞれ正負方向の制限開始速度、ωＰＴｒｍ及びωＭＴｒｍはそれぞれ正負方向の限界速度である。この例において、制限トルクＴＬｎｋは、各軸の制限開始位置と現在位置との差に基づいて計算される第１の制限トルクＴＬｎｋ_θと、制限開始速度と現在速度との差に基づいて計算される第２の制限トルクＴＬｎｋ_ωとの和である。
【００５１】
本来、これらのパラメータをロボット１の軸数分だけ設定する必要があるが、正負方向の位置による制限トルク計算定数ｋＰθ及びｋＭθ並びに正負方向の速度による制限トルク計算定数ｋＰω、ｋＭωは式(３)(４)(６)(７)により導出できるので、制限開始位置θＰＬｉｍ及びθＭＬｉｍ、限界位置θＰＴｒｍ及びθＭＴｒｍ、制限開始速度ωＰＬｉｍ及びωＭＬｉｍ、限界速度ωＰＴｒｍ及びωＭＴｒｍを教示装置３から入力するのみですべての定数を決定することができ、関節座標系における可動域制限トルクを計算することができる。
【００５２】
さらに、制限開始位置と制限開始速度を、それぞれ限界位置と限界速度から逆制限方向に一定量戻した値（すなわち、一定量だけ絶対値の小さな値）となるよう、当該一定量を事前に設定しておくことにより、Ｓ４１において操作者が教示装置３から設定する変数は、軸数分の正負方向の限界位置と限界速度、すなわち、軸数分のθＰＴｒｍ、θＭＴｒｍ、ωＰＴｒｍ、ωＭＴｒｍのみでよい。
【００５３】
なお、本実施例は、各軸の位置及び速度の両方に基づいて可動域制限トルクを計算してこれを反力として作用させるものであるが、このような構成に代えて、各軸の位置及び加速度に基づいた可動域制限を行うことも可能である。この場合、図３（ａ）において、各軸のモータのエンコーダにより検出される位置から加速度を計算する手段、加算部及び加速度フィードバックゲイン要素からなる加速度フィードバックループが構成される。
【００５４】
次に、図２のＳ１１において柔軟制御の座標系として作業座標系が指定された場合において実施される本発明の一実施例の可動域制限装置及び可動域制限方法の構成及び原理に関して、図４を用いて説明する。図４(ａ)は、作業座標系に基づきコントローラ２において可動域制限を実行する可動域制限付き位置制御部３０１のブロック図である。基本的な構成は図３（ａ）の構成に類似するが、図４（ａ）においては、順運動学計算部２１５において、関節位置指令に対して順運動学計算を行い、作業座標系への変換を行って、ロボットの手先位置に関する指令に変換する必要がある。作業座標系とはロボットの操作者が任意に決定できる座標系で、この例では作業座標系原点を手先位置として垂直方向にｚ軸、ロボット前後方向にｘ軸、ロボット左右方向にｙ軸をとっている。
【００５５】
以下、ロボット１の手先位置が作業座標系のｚ軸方向に移動する場合について動作を説明する。可動域制限付き位置制御部３０１において、ロボット１の手先位置は、軸（関節）１０１ａ〜１０１ｆに内蔵された各駆動モータ２０６のエンコーダ（図示せず）によって検出される各軸の位置、ロボット１の各リンクの既知の長さ、及びロボットアーム先端に取り付けられたエンドエフェクタ（手先効果器）の既知の寸法から得られる。エンドエフェクタは作業対象物に対して何らかの作業を行う装置であり、物を掴むハンド（グリッパ）やバリ取りを行うグラインダなど、用途によって様々なものが存在する。
【００５６】
得られた手先位置は、順運動学計算部２２１（各関節のモータの位置に基づいて作業座標系における手先位置を計算する）、加算部２１６及び位置フィードバックゲイン要素ＫｐＬＮＲ２１７からなる作業座標系位置フィードバックループと、順運動学計算部２２１、速度演算部２２０、加算部２１８及び速度フィードバックゲイン要素ＫｖＬＮＲ２１９からなる作業座標系速度フィードバックゲインループとによって、関節位置指令を順運動学計算部２１５により変換することにより得られる、作業座標系へ変換された手先位置指令にフィードバックされる。速度フィードバックゲイン要素ＫｖＬＮＲ２１９の出力は対応する力の値へと変換されて手先力制限部２１３へ入力されてもよい。ＫｖＬＮＲの乗算により、制御的に速度から力への単位変換が行われる。手先力制限部２１３は、ロボット１の手先力に対して予め与えられた制限（０％〜１００％）を与えるものであり、その制限値はロボット１の動作目的や動作環境等に応じて予め適切に定められる任意の値である。手先力制限部２１３を用いることにより、手先力を所定の力制限値内に留めることができる。
【００５７】
作業座標系で柔軟制御を実行する場合、上述のように手先力制限部２１３においてロボットの手先力が設定値以上にならないように制限を与えたり、位置フィードバックゲインＫｐＬＮＲや速度フィードバックゲインＫｖＬＮＲを可変にしたりすることが有用である。
【００５８】
可動域制限付き位置制御部３０１においては、さらに、後に詳述する手法により作業座標系における可動域制限力計算部２１２において計算された制限力を手先力制限部２１３の出力に加算した後、逆計算処理部２１４において逆計算を行って値を力から各軸（関節）に要求されるトルクに変換する。重力補償トルク計算部２０４及び摩擦補償トルク計算部２０５の動作は図３（ａ）の場合と同様である。
【００５９】
図４(ｂ)は、本実施例を６軸ロボットに適用したときの実施形態の説明図である。ロボット１の手先位置が作業座標系のｚ軸方向に移動する場合を例として説明する。図４（ｂ）に示されるＸＰＴｒｍｚは、手先の限界位置を示す。手先がこのＸＰＴｒｍｚまで移動することを回避するため、本発明では、限界位置ＸＰＴｒｍｚから一定量だけ戻した（すなわち、一定量だけ絶対値の小さな）位置（以下、「制限開始位置」という。）であるＸＰＬｉｍｚを予め設定しておく。そして、手先の位置が図４（ｂ）に示すように制限開始位置ＸＰＬｉｍｚを超えると、所定の計算手法に従って、図４（ａ）における作業座標系可動域制限力計算部２１２において反力ＦＬｎｒｚを計算し、これを加算して得た力に逆計算を施した後、得られたトルクを各軸のモータに指示する。例えば直接教示を行っている操作者は、それまで柔軟に制御できていたロボット１の動作が反力を受けたことを認識することができるので、ロボット１が可動域限界に近づいていることを認識することができる。
【００６０】
図４（ｃ)及び図４ (ｄ）は可動域制限力計算部２１２において実行される計算を説明する図である。図３に関連して既に説明した関節座標系の場合と同様に、反力は、ロボット１の手先の位置と速度の両方に基づいて計算される。図４（ｃ）において、縦軸は手先位置に基づいて決定される反力（制限力）を表し、横軸は手先位置を表す。図４（ｄ）において、縦軸は手先の移動する速度に基づいて決定される反力を表し、横軸は手先の移動する速度を表す。反力を滑らかに提示するよう、図４（ｃ）及び図４（ｄ）に示されるように、所定の制限開始位置（ＸＰＬｉｍ、ＸＭＬｉｍ）又は制限開始速度（ＶＰＬｉｍ、ＶＭＬｉｍ）から立ち上がり、限界位置（ＸＰＴｒｍ、ＸＭＴｒｍ）又は限界速度（ＶＰＴｒｍ、ＶＭＴｒｍ）に向かって漸近するような２次以上の曲線によって反力が表されるようにするのが好ましい。但し、反力と位置又は速度との関係は図４（ｃ）及び図４（ｄ）に示される関係に限定されない。反力は、手先の現在位置が制限開始位置を超えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて、大きくなるように規定されればよい。手先位置が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくとも一方を超えた場合に手先が限界位置又は限界速度に達することを回避するのに有用な様々な計算式を設定し得る。図４（ｃ）において、ＸＰＬｉｍ、ＸＭＬｉｍは、それぞれ正方向及び負方向の制限開始位置であり、ＸＰＴｒｍ、ＸＭＴｒｍは、それぞれ正方向及び負方向の限界位置である。図４（ｄ）に示される各値も、同様に、正方向及び負方向の制限開始速度並びに正方向及び負方向の限界速度をそれぞれ意味している。既に述べたように、制限開始位置及び制限開始速度は、それぞれ限界位置及び限界速度から所定の値だけ原点方向に近づいた値に設定される。
【００６１】
ここで再び図２に戻り、本発明の一実施例による作業座標系の場合の可動域制限の処理手順を説明する。
【００６２】
Ｓ２２において、作業座標系においてロボット１の手先が発揮する力の制限値を、例えば教示装置３から設定する。ここで設定された力制限は図４（ａ）における手先力制限部２１３において使用される。
【００６３】
次に、Ｓ３２において、作業座標系において柔軟制御を実行する。さらに、Ｓ４２において、作業座標系の可動域制限に必要となる制限力ＦＬｎｒ（図４参照）を以下に示す式(８)から式(１４)により計算するために必要となる各種のパラメータを設定する。この計算は、図４（ａ）の例においては、作業座標系可動域制限力計算部２１２において実行される。なお、制限力ＦＬｎｒの計算方法は、以下に示すものに限定されるものではなく、既に述べたとおり、手先位置が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくとも一方を超えた場合に手先が限界位置又は限界速度に達することを回避するのに有用な様々な計算手法を適用可能であることは当業者にとって明らかであろう。
【数２】
ここで、ＦＬｎｒ_ｘ及びＦＬｎｒ_ｖは、それぞれ、ロボット１の手先の位置に基づいて計算される制限力及び手先の速度に基づいて計算される制限力であり、例えば図４（ｃ）及び図４（ｄ）に示すような特性を有する。ｋＰｘ及びｋＭｘはそれぞれ正負方向の位置による制限力計算定数、ｋＰｖ及びｋＭｖはそれぞれ正負方向の速度による制限力計算定数である。ｘ及びｖは現在の手先位置及び速度、ｘＰＬｉｍ及びｘＭＬｉｍはそれぞれ正負方向の制限開始位置、ｘＰＴｒｍ及びｘＭＴｒｍはそれぞれ正負方向の限界位置である。ｖＰＬｉｍ及びｖＭＬｉｍはそれぞれ正負方向の制限開始速度、ｖＰＴｒｍ及びｖＭＴｒｍはそれぞれ正負方向の限界速度である。この例において、制限力ＦＬｎｒは、手先の制限開始位置と現在位置との差に基づいて計算される第１の制限力ＦＬｎｒ_ｘと、制限開始速度と現在速度との差に基づいて計算される第２の制限力ＦＬｎｒ_ｖとの和である。
【００６４】
本来、これらのパラメータを３次元作業座標系のＸ軸、Ｙ軸及びＺ軸の分だけ設定する必要があるが、正負方向の位置による制限力計算定数及び正負方向の速度による制限力計算定数は式（１０）（１１）（１３）（１４）により導出できるので、関節座標系の場合の設定と同様、制限開始位置ｘＰＬｉｍ及びｘＭＬｉｍ、限界位置ｘＰＴｒｍ及びｘＭＴｒｍ、制限開始速度ｖＰＬｉｍ及びｖＭＬｉｍ、限界速度ｖＰＴｒｍ及びｖＭＴｒｍを教示装置から入力するのみですべての定数を決定することができ、作業座標系における可動域制限力を計算することができる。さらに、制限開始位置及び制限開始速度を、それぞれ限界位置及び限界速度から逆制限方向に一定量戻した（すなわち、一定量だけ絶対値の小さな）値となるよう、当該一定量を事前に設定しておくことにより、Ｓ４２において操作者が教示装置３から設定する変数は、Ｘ軸、Ｙ軸及びＺ軸についての正負方向の限界位置及び限界速度、すなわち、ｘＰＴｒｍ、ｘＭＴｒｍ、ｖＰＴｒｍ、ｖＭＴｒｍのみでよい
ことになる。
【００６５】
作業座標系における可動域制限においては、図４（ａ）の逆計算処理部２１４が示すように、上述のようにして計算された制限力と手先力制限部２１３の出力との合計値を関節座標系の制限トルクに変換するために逆計算（２１４）を行う。作業座標系の手先力から関節座標系のトルクへの逆計算の方法としては、関節トルクと手先力の関係を規定するヤコビ逆行列を用いる方法などが一般的に知られている。
【００６６】
なお、本実施例は、ロボットの手先の位置及び速度の両方に基づいて可動域制限力を計算してこれを反力として作用させるものであるが、このような構成に代えて、手先の位置及び加速度に基づいた可動域制限を行うことも可能である。この場合、図４（ａ）において、順運動学計算部、加速度を計算する手段、加算部及び加速度フィードバックゲイン要素からなる加速度フィードバックループが構成される。
【００６７】
図２に示す本発明の実施例においては、関節座標系、作業座標系の場合とも、最後にＳ５１においてロボットのアーム端を操作者が直接操作してロボットに位置教示を行う。この場合、ロボットは柔軟制御状態にあり、通常の状態では、操作者の及ぼす外力に倣って動作することが出来る。そして、関節座標系の場合にはいずれかの軸が所定の制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれを超えたときに、作業座標系の場合には手先が所定の制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれを超えたときに、コントローラ２は、既に述べた本発明の実施例の手法によって、可動域制限トルク又は可動域制限力を発生させる。このような特徴により、本発明によれば、操作者は、ロボット１が現在可動域限界に近づきつつあることを認識することができ、ロボットの機械損傷を回避することができる。軸や手先の位置だけでなく速度にも基づいて可動域制限トルクや可動域制限力の計算を行うことにより、柔軟制御中にロボットに対して急激に大きな外力が働いた場合に、ロボット各軸や手先の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００６８】
直接教示は、ロボットの手先部等を直接操作して所望する位置に導き、位置登録するという過程で行われる。このような直接教示においては、操作者がロボットの動作位置に注視していなかったためにロボットが可動域限界に達してしまい、ロボットに機械損傷を与えたり、もしくは、ロボットを周辺装置と衝突させたりするという問題が起こり得る。本発明の実施例によれば、これらの問題を回避できる。すなわち、ロボットの動作領域を予め設定しておくことで、操作者の教示作業においてロボットが動作限界や周辺装置の付近に近付いた時に操作者に反力を提示することにより、上述のような機械損傷や衝突を防ぐことが出来るのである。
【００６９】
また、予め複数の周辺機器の配置情報を基にロボットの柔軟制御動作時の可動領域を必要最小限に制限しておくことで、ロボットがある周辺機器から外力を受けて想定以上に倣いそうになった場合でも、ロボットと別の周辺機器との干渉を回避できる。
【００７０】
また、本実施例では作業座標系の直交成分のみの説明であったが、回転成分でも同様の設定が可能であることは明白である。
【符号の説明】
【００７１】
１ ロボット
２ コントローラ
３ 教示装置
４ 操作者
１０１ａ〜ｆ ロボットの関節軸（６軸）
２０１ａ〜ｆ 柔軟制御状態での可動域制限付き位置制御部（６軸）
２０２ａ 関節座標系における可動域制限トルク計算部
２０３ａ トルク制限部
２０４、２０４ａ 重力補償トルク計算部
２０５、２０５ａ 摩擦補償トルク計算部
２０６ａ 駆動モータ
２０７ａ 速度演算部
２０８ａ 加算部
２０９ａ 位置フィードバックゲイン要素
２１０ａ 加算部
２１１ａ 速度フィードバックゲイン要素
２１２ 作業座標系における可動域制限力計算部
２１３ 手先力制限部
２１４ 逆計算処理部
２１５ 順運動学計算部
２１６ 加算部
２１７ 位置フィードバックゲイン要素
２１８ 加算部
２１９ 速度フィードバックゲイン要素
２２０ 速度演算部
２２１ 順運動学計算部
２２２ 加算部
【技術分野】
【０００１】
本発明の実施例は、ロボットの制御装置及び方法に関し、例えばロボットの柔軟制御における可動域制限装置及び方法に関する。
【背景技術】
【０００２】
産業用ロボットの柔軟制御は、力センサなどを使用することなくロボットアームを直接操作して位置教示を行う場合や、周辺機器がロボットに外力を及ぼす状況下でのハンドリング作業などにおいて使用される。この柔軟制御を用いて作業する場合、操作者や周辺機器からの外力によってロボットがその可動域を超えて無制限に動作されることにより、ロボットのメカリミットの破損やロボットと別の周辺機器との干渉が発生する危険性があるため、何らかの方法で可動領域を制限する必要がある。
【０００３】
従来の可動域制限方法で一般に知られているものとして、ロボットコントローラに実装されたソフトウェア上でロボットアーム各軸のモータにつき動作制限位置を予め設定しておき、いずれかの軸のモータが当該動作制限位置に達した場合にアラームを発生し、ロボットを緊急停止させる手法がある。
【０００４】
また、前記手法と同様に、ソフトウェア上でロボットアームの各軸のモータについて動作制限位置を予め設定しておき、各軸の位置（関節角度、つまりモータ回転角度である）が当該動作制限位置に近づくにつれて漸増する力を所定の方法で計算し、計算された力を反力として加えることにより、各軸の動きが可動域を超えることを回避する手法がある（例えば、特許文献１参照）。
【０００５】
さらに、機構的な対処法として、ロボットの各軸の位置が機械的動作限界へ近づくにつれて大きな抵抗値を示すようになる抵抗部材をロボットの関節部に設けるものもある（例えば、特許文献２参照）。
【０００６】
図５は、特許文献１に記載の手法をマスタースレーブマニピュレータに適用し動作限界の反力提示を行う制御装置を示すブロック図である。スレーブアーム側の関節角度Ｐｓが所定の作動限界近傍に達すると、その関節角度Ｐｓに応じて計算される反力ＴＬをマスターアームの反力に足しこむように構成されている。図６はスレーブアームの関節角度Ｐｓと外力に加える反力ＴＬとの関係について予め設定される関係を示す。図６(ａ)は関節角度Ｐｓに関係なくＴＬ＝０とするものである。図６（ｂ）は、反力ＴＬが関節角度Ｐｓと所定の作動限界近傍の制限値ＰＬＬとの差に比例して増加するように与えられる例を示す。図６（ｃ）は、ＰｓとＰＬＬとの差の２乗に比例するように反力ＴＬを決定する例を示す。
【０００７】
このように、従来のロボットアームの可動域制限方法では、有力な手法として、ロボットアームの各軸について所定の制限位置（関節角度）と現在位置との差から反力を計算し、当該反力を加えることによって各軸の動作が可動域を超えることが回避されていた。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開平１−１８３３７９号公報
【特許文献２】実公昭６２−０３１２６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
各軸について動作制限位置を設けロボットアーム各軸の現在位置を常時監視する従来の方法は、いずれかの軸が制限位置に到達した時にロボットアームを緊急停止させるという手順をとっているので、復旧に時間や手間がかかるという問題がある。
【００１０】
また、各軸に関して予め設定された関節制限位置と現在位置との差から計算される反力を加える方法においては、反力がロボットの各軸の位置のみに基づいて計算されるため、柔軟制御中に急激に大きな外乱トルクが印加されていずれかの軸の位置が制限領域に到達した場合や高速で制限領域に到達した場合には、逆向きのトルクが過大となり、減速機などに大きな負担がかかるという問題があった。
【００１１】
さらに、機構的な対処法には、各軸が繰り返し制限領域に近づくような場合に制限機構が劣化し易いという問題があった。
【００１２】
したがって、従来技術によると、ロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合には、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができなかった。
【課題を解決するための手段】
【００１３】
上記問題を解決するため、本願は、次のような発明を提供する。
【００１４】
なお、本願出願時の明細書の記載を基礎として、特許請求の範囲の記載が補正され得ることは勿論であるが、その際に、本願発明に関して記載される以下の概要の内容を訂正することは予定しない。以下の概要に記載される各「発明」は、本願明細書における開示の要点としての意味を持つものである。
【００１５】
本発明の１つの側面において、本発明の制御装置は、少なくとも１つの関節を有するロボットの柔軟制御に使用されるものであり、各関節につき、関節位置指令に対して、該関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算する制限トルク計算手段であって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限トルクが大きくなる、制限トルク計算手段とを備える。そして、当該制御装置は、速度フィードバック制御手段の出力に計算された制限トルクを加算して得られるトルクにより該関節の動作を制御する。
【００１６】
制限トルクは、制限開始位置と現在位置との差に基づいて計算される第１の制限トルクと、制限開始速度と現在速度との差に基づいて計算される第２の制限トルクとの和として計算されてもよい。当該制限トルクは、本明細書において後述する式(１)から式(７)に基づいて計算してもよい。
【００１７】
このように構成することによって、関節座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００１８】
また、本発明の別の側面において、本発明の制御装置は、少なくとも１つの関節を有するロボットの柔軟制御に使用されるものであり、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定する順運動学計算部と、手先位置指令に対して、各関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、作業座標系において、手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と手先の現在位置との関係、及び手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と手先の現在速度との関係に基づいて、制限力を計算する制限力計算手段であって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限力が大きくなる、制限力計算手段と、計算された制限力を速度フィードバック制御手段の出力に加算して得られる力から各関節に要求されるトルクを計算する逆計算手段とを備える。そして、当該制御装置は、計算されたトルクにより各関節の動作を制御する。
【００１９】
制限力は、制限開始位置と現在位置との差に基づいて計算される第１の制限力と制限開始速度と現在速度との差に基づいて計算される第２の制限力との和として計算されてもよい。当該制限力は、本明細書において後述する式(８)から式(１４)に基づいて計算してもよい。
【００２０】
このように構成することによって、作業座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２１】
また、本発明の別の側面において、本発明の制御方法は、少なくとも１つの関節を有するロボットの柔軟制御に使用され、少なくとも１つの関節につき、関節位置指令に対して、該関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算するステップであって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限トルクが大きくなる、制限トルクを計算するステップと、計算された制限トルクを速度フィードバック制御の出力に加算して得られるトルクにより該関節の動作を制御するステップとを備える。
【００２２】
このように構成することによって、関節座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２３】
また、本発明の別の側面において、本発明の制御方法は、少なくとも１つの関節を有するロボットの柔軟制御に使用され、順運動学計算により、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定するステップと、手先位置指令に対して、各関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、作業座標系において、手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と手先の現在位置との関係、及び手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と手先の現在速度との関係に基づいて、制限力を計算するステップであって、現在位置が制限開始位置を越えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて制限力が大きくなる、制限力を計算するステップと、逆計算により、計算された制限力を速度フィードバック制御の出力に加算して得られる力から各関節に要求されるトルクを計算するステップと、計算されたトルクにより各関節の動作を制御するステップとを備える。
【００２４】
このように構成することによって、作業座標系上でのロボットの柔軟制御中にロボットに対して急激に大きな外力が働いた場合においても、ロボット各軸の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００２５】
審査中の手続補正によって、及び、特許後の訂正審判又は訂正請求において、法的な制限の範囲内で、本発明は、種々の他の態様に訂正され得る。
【００２６】
なお、特許後の訂正審判又は訂正請求における「実質上特許請求の範囲を変更」の判断は、特許時の請求項に新たな構成要素が追加されたか否か（すなわち、いわゆる外的付加がなされたか否か）、又は、特許時の請求項の１つ又はそれより多いいずれかの構成要素をさらに限定するものか（すなわち、いわゆる内的付加がなされたか）によって判断されるべきでなく、訂正の前後の請求項に係る発明の効果が類似するか否かの観点からなされるべきである。
【発明の効果】
【００２７】
本発明の実施例によると、関節座標系上でロボットの柔軟制御を実行した際に、ロボットが可動域限界に到達したときにロボットを緊急停止することなく機械的可動域を保護し、周辺機器への干渉回避を行うことができる。
【００２８】
また、作業座標系上でロボットの柔軟制御を実行した際に、ロボットが可動域限界に到達したときにロボットを緊急停止することなく機械的可動域を保護し、周辺機器への干渉回避を行うことができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の一実施例によるロボットシステムの構成図である。
【図２】本発明の一実施例の装置又は方法の処理手順を示すフローチャートである。
【図３】本発明の一実施例による可動域制限装置又は方法を関節座標系での柔軟制御に適用した場合の説明図である。
【図４】本発明の一実施例による可動域制限装置又は方法を作業座標系での柔軟制御に適用した場合の説明図である。
【図５】従来の方法を適用したマスタースレーブマニピュレータの制御ブロック図である。
【図６】図５に示す従来の方法における反力ＴＬと関節角度Ｐｓとの関係を説明するグラフである。
【発明を実施するための形態】
【００３０】
以下、本発明の装置及び方法の具体的実施例について、図に基づいて説明する。
【００３１】
ここでは、本発明の一実施例として、本発明の装置又は方法をロボットの直接教示（ダイレクトティーチング）に適用した場合について説明する。ここで、教示（ティーチング）とは、ロボットに作業を行わせるためにその作業を実現する動作を何らかの方法で教え込むことを意味する。直接教示とは、教示装置を介することなく、操作者がある作業のための所望の動作手順に沿ってロボットに力を加えて手動でロボットを動かすことにより、当該動作をロボットに教え込む教示手法である。
【００３２】
図１は、本発明の可動域制限装置及び方法が実施されるロボットシステムの概略的な構成図を示す。図１において、ロボットシステムは、６軸の垂直多関節型ロボット１、本発明の可動域制限装置の実施例であり、本発明の可動域制限方法の実施例を実現するコントローラ２、及び教示の際に各種設定を行うための教示装置３を備えている。図１においては、ロボットアームの直接教示を行う操作者４も図示されている。図１においてロボット１は６軸の垂直多関節型を例にしているが、これに限定されない。本発明は、少なくとも１つの関節を有する様々なロボットに対して適用することが可能である。教示装置３は、コントローラ２と有線又は無線で接続されており、表示画面及び複数のボタンを備え、操作者４により把持されて操作される。教示装置３は、ロボット１に所定の作業を行うために必要となる動作を教示したり、ロボット１の状態を把握したり、教示した作業の開始、一時停止、非常停止などを指令したりするのに使用される。教示装置３は、教示ペンダント、ティーチングボックス、ティーチングペンダント、プログラミングペンダント等とも呼称される。教示装置３を用いた操作の一例として、ジョグ運転と呼ばれる操作を行ってもよい。ジョグ運転とは、操作者４が教示装置３上の各軸動作ボタンを押している間だけロボット１の所定の軸が動作し、放すと動作が停止するという操作である。操作者４は、ジョグ運転により、ロボット１を所望の位置や姿勢へと動かし、教示装置３を介して教示操作を行うことにより、そのときのロボット１の位置及び姿勢をコントローラ２に記憶させることができる。
【００３３】
ロボット１の柔軟制御を行う場合、通常の状態では、ロボットアームは外力に倣って動作する。柔軟制御とは、ロボットアームに作用する重力や摩擦力等のみを補償するトルク指令を与え、ロボットアームを外力に倣って動作させる制御方法である。
【００３４】
後述するように、本発明によれば、操作者４が教示装置３を介して可動域制限に関するパラメータをコントローラ２に登録しておくと、ロボット１の各軸（各関節）の動作が可動域限界に近づいた場合にコントローラ２が適切な反力を計算して各軸に印加する。これにより、各軸の位置（回転角度）が可動域限界に達することを回避することができる。
【００３５】
図２は、コントローラ２において実行される可動域制限の処理手順を示すフローチャートである。以下、図２を用いるとともに必要に応じて図３及び図４を参照しながら処理手順を順を追って説明する。
【００３６】
はじめに、Ｓ１１において、柔軟制御を実行する座標系について関節座標系か作業座標系かを選択する。ここで、関節座標系とは、ロボットの各軸に固定された座標系であり、ロボットを動作させる際の基準として使用される座標系を意味する。また、作業座標系とは、ロボットの手先部に固定された座標系であり、直交座標系とも呼称される。ロボットの柔軟制御は、関節座標系に基づいて行われる制御と作業座標系に基づいて行われる制御とに大別されるため、Ｓ１１においていずれの座標系に基づく柔軟制御を実行するかを選択する。当該選択は、制御対象のロボット１に関してコントローラ２が予め記憶している情報に基づいてコントローラ２が行ってもよいし、操作者４が教示装置３を介してコントローラ２へ選択の指示を送ることにより行われてもよい。その他、当業者に周知の様々な手法により、コントローラ２はＳ１１を実行することができる。
【００３７】
ここで、Ｓ１１において柔軟制御の座標系として関節座標系が指定された場合において実施される本発明の一実施例の可動域制限装置及び可動域制限方法の構成及び原理に関して、図３を用いて説明する。図３（ａ）は、関節座標系に基づきコントローラ２において可動域制限を実行する可動域制限付き位置制御部２０１のブロック図である。
【００３８】
図３（ａ）においては、２０１ａ乃至２０１ｃの３つのブロック図が示されており、添え字のアルファベットはロボットの各軸に対応する。可動域制限はロボット１の各軸（各関節）ごとに行う必要がある。すなわち、６軸を有するロボットであれば６つの軸に対してそれぞれ同様の制御を行う必要がある。
【００３９】
以下、軸ａに対応する可動域制限付き位置制御部２０１ａを例にとり動作を説明する。図３（ａ）に示される関節位置指令は、ロボット１の手先を移動させるにあたって各軸に要求される関節位置（回転角度）を示す指令である。操作者が教示装置３を使用してロボット１にある作業に関する動作を教え込む教示の際には、関節位置指令は操作者による教示装置３への入力に基づいて、コントローラ２へ送られる。既に教示された動作をロボット１が実行する再生動作の場合には、関節位置指令はコントローラ２から得られる。また、直接教示の場合には、操作者がロボットを直接操作し、所望する位置に導く。この際、教示装置３は直接教示に関する数値設定や設定切換えに使用される。このような直接教示の際には、操作者がロボットに印加した力に基づいて各関節に位置指令が与えられることになる。一般的に柔軟制御は周辺機器がロボットに外力を及ぼす状況下でのハンドリング作業などにおいて使用され、外力を吸収するという受動的な使用形態をとる。本実施例では直接教示の際に柔軟制御を適用しているが、ハンドリング作業などの際に本発明を適用することももちろん可能である。再生（playback）とは、ロボットが、記憶した情報によって指示された作業・動作を実行することを意味する。
【００４０】
可動域制限付き位置制御部２０１ａにおいて、軸ａの位置は、軸ａに内蔵された駆動モータ２０６ａのエンコーダ（図示せず）によって検出される。エンコーダは、モータの位置（回転角度）を測定するためのセンサである。エンコーダの出力を時間微分するとモータの回転（角）速度が得られる。ロボット制御において、エンコーダは、通常、各軸を駆動するモータに設けられており、エンコーダにより測定された各軸の位置（回転角度）とロボットの各リンク（軸と軸とを繋ぐ部分）の既知の長さとからロボットの手先の位置及び姿勢を求めることができる。このようにしてロボットの手先の位置及び姿勢を求めることを順運動学という。
【００４１】
エンコーダ（図示せず）によって検出された軸ａの位置は、加算部２０８ａ及び位置フィードバックゲイン要素Ｋｐ２０９ａからなる、位置フィードバック制御を行う関節座標系位置フィードバックループと、速度演算部２０７ａ、加算部２１０ａ及び速度フィードバックゲイン要素Ｋｖ２１１ａからなる、速度フィードバック制御を行う関節座標系速度フィードバックループとによって、関節位置指令にフィードバックされる。速度フィードバックゲイン要素Ｋｖ２１１ａの出力は対応するトルクの値へと変換されて、トルク制限部２０３ａへ入力されてもよい。Ｋｖの乗算により、制御的に速度からトルクへの単位変換が行われる。Ｋｐの出力及びＫｖの入力は関節速度指令となる。トルク制限部２０３ａは、ロボット１の軸ａのモータの発生トルクに対して予め与えられた制限（０％〜１００％）を与えるものであり、その制限値はロボット１の動作目的や動作環境等に応じて予め適切に定められる任意の値である。トルク制限部２０３ａを用いることにより、軸ａのモータのトルクを所定のトルク制限値内に留めることができる。
【００４２】
関節座標系で柔軟制御を実行する場合、上述のようにトルク制限部２０３においてロボットの各軸のモータの発生トルクが設定値以上にならないように制限を与えたり、位置フィードバックゲインＫｐや速度フィードバックゲインＫｖを可変にしたりすることが有用である。
【００４３】
重力補償トルク計算部２０４ａ及び摩擦補償トルク計算部２０５ａは、それぞれ、重量や重心位置などのロボット１に関するリンクパラメータに基づいて重力補償トルクを計算し、摩擦モデルから速度に応じた摩擦補償トルクを計算する。
【００４４】
図３(ｂ)は、ａ〜ｆの６軸を有するロボット１の概略図であり、各軸と、軸同士を接続するリンクとが示されている。ａ〜ｆの各軸は限界位置（限界回転角度）を有しており、これを超えて回転がなされると故障等につながる。
【００４５】
軸１０１ｂを例として説明する。図３（ｂ）に示されるθＰＴｒｍｂは、軸ｂの限界位置（限界回転角度）を示す。軸ｂがこのθＰＴｒｍｂまで回転することを回避するため、本発明では、限界位置θＰＴｒｍｂから一定量だけ戻した（すなわち、一定量だけ絶対値の小さな）位置（以下、「制限開始位置」という。）であるθＰＬｉｍｂを予め決定しておく。そして、軸ｂの位置が図３（ｂ）に示すように制限開始位置θＰＬｉｍｂに達するかもしくはこれを超えると、所定の計算手法に従って、図３（ａ）における関節座標系可動域制限トルク計算部２０２ｂ（図３（ａ）における軸ｂの可動域制限トルク計算部である。軸ｂに対応する可動域制限付き位置制御部２０１ｂに存在する）において反力ＴＬｎｋｂを計算し、これを加算して得たトルクを軸１０１ｂのモータに指示する。例えば直接教示（ダイレクトティーチング）を行っている操作者は、それまで柔軟に制御できていたロボット１の動作が反力を受けたことを認識することができるので、ロボット１が可動域限界に近づいていることを認識することができる。なお、図中のその他の軸に関しても反力を記しているが、太線、細線、点線となるに従って反力が弱くなることを意味している。
【００４６】
図３(ｃ)及び図３(ｄ)は可動域制限トルク計算部２０２において実行される計算を説明する図である。図３（ｃ）において、縦軸は計算対象の軸の位置（回転角度）に基づいて決定される反力（制限トルク）を表し、横軸は計算対象の軸の位置を表す。図３（ｄ）において、縦軸は計算対象の軸の速度（角速度）に基づいて決定される反力を表し、横軸は計算対象の軸の速度（角速度）を表す。反力を滑らかに提示するよう、図３（ｃ）及び図３（ｄ）に示すように、制限開始位置（θＰＬｉｍ、θＭＬｉｍ）及び制限開始速度（ωＰＬｉｍ、ωＭＬｉｍ）から立ち上がり、限界位置（θＰＴｒｍ、θＭＴｒｍ）及び限界速度（ωＰＴｒｍ、ωＭＴｒｍ）に漸近するような２次以上の曲線によって反力が表されるようにするのが好ましい。但し、反力と位置又は速度との関係は図３（ｃ）及び図３（ｄ）に示される関係に限定されない。反力は、軸の現在位置が制限開始位置を超えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて、大きくなるように規定されればよい。軸が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくともいずれかを超えた場合に限界位置又は限界速度に達することを回避するのに有用な様々な計算式を設定し得る。図３（ｃ）において、θＭＬｉｍ及びθＰＬｉｍは、それぞれ負方向及び正方向の制限開始位置であり、θＭＴｒｍ及びθＰＴｒｍは、それぞれ負方向及び正方向の限界位置である。図３（ｄ）に示される各値も、同様に、負方向及び正方向の制限開始速度並びに負方向及び正方向の限界速度をそれぞれ意味している。既に述べたように、制限開始位置及び制限開始速度は、それぞれ限界位置及び限界速度から所定の値だけ原点方向に近づいた値に設定される。
【００４７】
ここで再び図２に戻り、本発明の一実施例による関節座標系の場合の可動域制限の処理手順を説明する。
【００４８】
Ｓ２１において、関節座標系においてロボットの各軸に配置されたモータの発揮するトルクの制限値を、例えば教示装置３から設定する。ここで設定されたトルク制限は図３（ａ）に示すトルク制限部２０３において使用される。
【００４９】
次に、Ｓ３１において、関節座標系において柔軟制御を開始する。すなわち、ロボット１のアームが外力に倣って動作するような状態にロボット１を設定する。
【００５０】
さらに、Ｓ４１において、関節座標系の可動域制限に必要となる制限トルクＴＬｎｋ（図３参照）を、例えば以下に示す式(１)から式(７)により計算するために必要となる各種のパラメータを設定する。この計算は、図３（ａ）の例においては、関節座標系可動域制限トルク計算部２０２ａにおいて実行される。なお、制限トルクＴＬｎｋの計算方法は、以下に示すものに限定されるものではなく、既に述べたとおり、各軸（各関節）の動作が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくともいずれかを超えた場合に限界位置又は限界速度に達することを回避するのに有用な様々な計算手法を適用可能であることは当業者にとって明らかであろう。
【数１】
ここで、ＴＬｎｋ_θ及びＴＬｎｋ_ωは、それぞれ、各軸の位置（回転角度）に基づいて計算される制限トルク及び各軸の速度（角速度）に基づいて計算される制限トルクであり、例えば図３（ｃ）及び図３（ｄ）に示すような特性を有する。ｋＰθ及びｋＭθはそれぞれ正負方向の位置による制限トルク計算定数、ｋＰω及びｋＭωはそれぞれ正負方向の速度による制限トルク計算定数である。θ及びωは現在の位置（回転角度、関節角度）及び速度（角速度）、θＰＬｉｍ及びθＭＬｉｍはそれぞれ正負方向の制限開始位置、θＰＴｒｍ及びθＭＴｒｍはそれぞれ正負方向の限界位置である。ωＰＬｉｍ及びωＭＬｉｍはそれぞれ正負方向の制限開始速度、ωＰＴｒｍ及びωＭＴｒｍはそれぞれ正負方向の限界速度である。この例において、制限トルクＴＬｎｋは、各軸の制限開始位置と現在位置との差に基づいて計算される第１の制限トルクＴＬｎｋ_θと、制限開始速度と現在速度との差に基づいて計算される第２の制限トルクＴＬｎｋ_ωとの和である。
【００５１】
本来、これらのパラメータをロボット１の軸数分だけ設定する必要があるが、正負方向の位置による制限トルク計算定数ｋＰθ及びｋＭθ並びに正負方向の速度による制限トルク計算定数ｋＰω、ｋＭωは式(３)(４)(６)(７)により導出できるので、制限開始位置θＰＬｉｍ及びθＭＬｉｍ、限界位置θＰＴｒｍ及びθＭＴｒｍ、制限開始速度ωＰＬｉｍ及びωＭＬｉｍ、限界速度ωＰＴｒｍ及びωＭＴｒｍを教示装置３から入力するのみですべての定数を決定することができ、関節座標系における可動域制限トルクを計算することができる。
【００５２】
さらに、制限開始位置と制限開始速度を、それぞれ限界位置と限界速度から逆制限方向に一定量戻した値（すなわち、一定量だけ絶対値の小さな値）となるよう、当該一定量を事前に設定しておくことにより、Ｓ４１において操作者が教示装置３から設定する変数は、軸数分の正負方向の限界位置と限界速度、すなわち、軸数分のθＰＴｒｍ、θＭＴｒｍ、ωＰＴｒｍ、ωＭＴｒｍのみでよい。
【００５３】
なお、本実施例は、各軸の位置及び速度の両方に基づいて可動域制限トルクを計算してこれを反力として作用させるものであるが、このような構成に代えて、各軸の位置及び加速度に基づいた可動域制限を行うことも可能である。この場合、図３（ａ）において、各軸のモータのエンコーダにより検出される位置から加速度を計算する手段、加算部及び加速度フィードバックゲイン要素からなる加速度フィードバックループが構成される。
【００５４】
次に、図２のＳ１１において柔軟制御の座標系として作業座標系が指定された場合において実施される本発明の一実施例の可動域制限装置及び可動域制限方法の構成及び原理に関して、図４を用いて説明する。図４(ａ)は、作業座標系に基づきコントローラ２において可動域制限を実行する可動域制限付き位置制御部３０１のブロック図である。基本的な構成は図３（ａ）の構成に類似するが、図４（ａ）においては、順運動学計算部２１５において、関節位置指令に対して順運動学計算を行い、作業座標系への変換を行って、ロボットの手先位置に関する指令に変換する必要がある。作業座標系とはロボットの操作者が任意に決定できる座標系で、この例では作業座標系原点を手先位置として垂直方向にｚ軸、ロボット前後方向にｘ軸、ロボット左右方向にｙ軸をとっている。
【００５５】
以下、ロボット１の手先位置が作業座標系のｚ軸方向に移動する場合について動作を説明する。可動域制限付き位置制御部３０１において、ロボット１の手先位置は、軸（関節）１０１ａ〜１０１ｆに内蔵された各駆動モータ２０６のエンコーダ（図示せず）によって検出される各軸の位置、ロボット１の各リンクの既知の長さ、及びロボットアーム先端に取り付けられたエンドエフェクタ（手先効果器）の既知の寸法から得られる。エンドエフェクタは作業対象物に対して何らかの作業を行う装置であり、物を掴むハンド（グリッパ）やバリ取りを行うグラインダなど、用途によって様々なものが存在する。
【００５６】
得られた手先位置は、順運動学計算部２２１（各関節のモータの位置に基づいて作業座標系における手先位置を計算する）、加算部２１６及び位置フィードバックゲイン要素ＫｐＬＮＲ２１７からなる作業座標系位置フィードバックループと、順運動学計算部２２１、速度演算部２２０、加算部２１８及び速度フィードバックゲイン要素ＫｖＬＮＲ２１９からなる作業座標系速度フィードバックゲインループとによって、関節位置指令を順運動学計算部２１５により変換することにより得られる、作業座標系へ変換された手先位置指令にフィードバックされる。速度フィードバックゲイン要素ＫｖＬＮＲ２１９の出力は対応する力の値へと変換されて手先力制限部２１３へ入力されてもよい。ＫｖＬＮＲの乗算により、制御的に速度から力への単位変換が行われる。手先力制限部２１３は、ロボット１の手先力に対して予め与えられた制限（０％〜１００％）を与えるものであり、その制限値はロボット１の動作目的や動作環境等に応じて予め適切に定められる任意の値である。手先力制限部２１３を用いることにより、手先力を所定の力制限値内に留めることができる。
【００５７】
作業座標系で柔軟制御を実行する場合、上述のように手先力制限部２１３においてロボットの手先力が設定値以上にならないように制限を与えたり、位置フィードバックゲインＫｐＬＮＲや速度フィードバックゲインＫｖＬＮＲを可変にしたりすることが有用である。
【００５８】
可動域制限付き位置制御部３０１においては、さらに、後に詳述する手法により作業座標系における可動域制限力計算部２１２において計算された制限力を手先力制限部２１３の出力に加算した後、逆計算処理部２１４において逆計算を行って値を力から各軸（関節）に要求されるトルクに変換する。重力補償トルク計算部２０４及び摩擦補償トルク計算部２０５の動作は図３（ａ）の場合と同様である。
【００５９】
図４(ｂ)は、本実施例を６軸ロボットに適用したときの実施形態の説明図である。ロボット１の手先位置が作業座標系のｚ軸方向に移動する場合を例として説明する。図４（ｂ）に示されるＸＰＴｒｍｚは、手先の限界位置を示す。手先がこのＸＰＴｒｍｚまで移動することを回避するため、本発明では、限界位置ＸＰＴｒｍｚから一定量だけ戻した（すなわち、一定量だけ絶対値の小さな）位置（以下、「制限開始位置」という。）であるＸＰＬｉｍｚを予め設定しておく。そして、手先の位置が図４（ｂ）に示すように制限開始位置ＸＰＬｉｍｚを超えると、所定の計算手法に従って、図４（ａ）における作業座標系可動域制限力計算部２１２において反力ＦＬｎｒｚを計算し、これを加算して得た力に逆計算を施した後、得られたトルクを各軸のモータに指示する。例えば直接教示を行っている操作者は、それまで柔軟に制御できていたロボット１の動作が反力を受けたことを認識することができるので、ロボット１が可動域限界に近づいていることを認識することができる。
【００６０】
図４（ｃ)及び図４ (ｄ）は可動域制限力計算部２１２において実行される計算を説明する図である。図３に関連して既に説明した関節座標系の場合と同様に、反力は、ロボット１の手先の位置と速度の両方に基づいて計算される。図４（ｃ）において、縦軸は手先位置に基づいて決定される反力（制限力）を表し、横軸は手先位置を表す。図４（ｄ）において、縦軸は手先の移動する速度に基づいて決定される反力を表し、横軸は手先の移動する速度を表す。反力を滑らかに提示するよう、図４（ｃ）及び図４（ｄ）に示されるように、所定の制限開始位置（ＸＰＬｉｍ、ＸＭＬｉｍ）又は制限開始速度（ＶＰＬｉｍ、ＶＭＬｉｍ）から立ち上がり、限界位置（ＸＰＴｒｍ、ＸＭＴｒｍ）又は限界速度（ＶＰＴｒｍ、ＶＭＴｒｍ）に向かって漸近するような２次以上の曲線によって反力が表されるようにするのが好ましい。但し、反力と位置又は速度との関係は図４（ｃ）及び図４（ｄ）に示される関係に限定されない。反力は、手先の現在位置が制限開始位置を超えて大きくなるにつれて又は現在速度が制限開始速度を超えて大きくなるにつれて、大きくなるように規定されればよい。手先位置が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくとも一方を超えた場合に手先が限界位置又は限界速度に達することを回避するのに有用な様々な計算式を設定し得る。図４（ｃ）において、ＸＰＬｉｍ、ＸＭＬｉｍは、それぞれ正方向及び負方向の制限開始位置であり、ＸＰＴｒｍ、ＸＭＴｒｍは、それぞれ正方向及び負方向の限界位置である。図４（ｄ）に示される各値も、同様に、正方向及び負方向の制限開始速度並びに正方向及び負方向の限界速度をそれぞれ意味している。既に述べたように、制限開始位置及び制限開始速度は、それぞれ限界位置及び限界速度から所定の値だけ原点方向に近づいた値に設定される。
【００６１】
ここで再び図２に戻り、本発明の一実施例による作業座標系の場合の可動域制限の処理手順を説明する。
【００６２】
Ｓ２２において、作業座標系においてロボット１の手先が発揮する力の制限値を、例えば教示装置３から設定する。ここで設定された力制限は図４（ａ）における手先力制限部２１３において使用される。
【００６３】
次に、Ｓ３２において、作業座標系において柔軟制御を実行する。さらに、Ｓ４２において、作業座標系の可動域制限に必要となる制限力ＦＬｎｒ（図４参照）を以下に示す式(８)から式(１４)により計算するために必要となる各種のパラメータを設定する。この計算は、図４（ａ）の例においては、作業座標系可動域制限力計算部２１２において実行される。なお、制限力ＦＬｎｒの計算方法は、以下に示すものに限定されるものではなく、既に述べたとおり、手先位置が制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれらの少なくとも一方を超えた場合に手先が限界位置又は限界速度に達することを回避するのに有用な様々な計算手法を適用可能であることは当業者にとって明らかであろう。
【数２】
ここで、ＦＬｎｒ_ｘ及びＦＬｎｒ_ｖは、それぞれ、ロボット１の手先の位置に基づいて計算される制限力及び手先の速度に基づいて計算される制限力であり、例えば図４（ｃ）及び図４（ｄ）に示すような特性を有する。ｋＰｘ及びｋＭｘはそれぞれ正負方向の位置による制限力計算定数、ｋＰｖ及びｋＭｖはそれぞれ正負方向の速度による制限力計算定数である。ｘ及びｖは現在の手先位置及び速度、ｘＰＬｉｍ及びｘＭＬｉｍはそれぞれ正負方向の制限開始位置、ｘＰＴｒｍ及びｘＭＴｒｍはそれぞれ正負方向の限界位置である。ｖＰＬｉｍ及びｖＭＬｉｍはそれぞれ正負方向の制限開始速度、ｖＰＴｒｍ及びｖＭＴｒｍはそれぞれ正負方向の限界速度である。この例において、制限力ＦＬｎｒは、手先の制限開始位置と現在位置との差に基づいて計算される第１の制限力ＦＬｎｒ_ｘと、制限開始速度と現在速度との差に基づいて計算される第２の制限力ＦＬｎｒ_ｖとの和である。
【００６４】
本来、これらのパラメータを３次元作業座標系のＸ軸、Ｙ軸及びＺ軸の分だけ設定する必要があるが、正負方向の位置による制限力計算定数及び正負方向の速度による制限力計算定数は式（１０）（１１）（１３）（１４）により導出できるので、関節座標系の場合の設定と同様、制限開始位置ｘＰＬｉｍ及びｘＭＬｉｍ、限界位置ｘＰＴｒｍ及びｘＭＴｒｍ、制限開始速度ｖＰＬｉｍ及びｖＭＬｉｍ、限界速度ｖＰＴｒｍ及びｖＭＴｒｍを教示装置から入力するのみですべての定数を決定することができ、作業座標系における可動域制限力を計算することができる。さらに、制限開始位置及び制限開始速度を、それぞれ限界位置及び限界速度から逆制限方向に一定量戻した（すなわち、一定量だけ絶対値の小さな）値となるよう、当該一定量を事前に設定しておくことにより、Ｓ４２において操作者が教示装置３から設定する変数は、Ｘ軸、Ｙ軸及びＺ軸についての正負方向の限界位置及び限界速度、すなわち、ｘＰＴｒｍ、ｘＭＴｒｍ、ｖＰＴｒｍ、ｖＭＴｒｍのみでよい
ことになる。
【００６５】
作業座標系における可動域制限においては、図４（ａ）の逆計算処理部２１４が示すように、上述のようにして計算された制限力と手先力制限部２１３の出力との合計値を関節座標系の制限トルクに変換するために逆計算（２１４）を行う。作業座標系の手先力から関節座標系のトルクへの逆計算の方法としては、関節トルクと手先力の関係を規定するヤコビ逆行列を用いる方法などが一般的に知られている。
【００６６】
なお、本実施例は、ロボットの手先の位置及び速度の両方に基づいて可動域制限力を計算してこれを反力として作用させるものであるが、このような構成に代えて、手先の位置及び加速度に基づいた可動域制限を行うことも可能である。この場合、図４（ａ）において、順運動学計算部、加速度を計算する手段、加算部及び加速度フィードバックゲイン要素からなる加速度フィードバックループが構成される。
【００６７】
図２に示す本発明の実施例においては、関節座標系、作業座標系の場合とも、最後にＳ５１においてロボットのアーム端を操作者が直接操作してロボットに位置教示を行う。この場合、ロボットは柔軟制御状態にあり、通常の状態では、操作者の及ぼす外力に倣って動作することが出来る。そして、関節座標系の場合にはいずれかの軸が所定の制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれを超えたときに、作業座標系の場合には手先が所定の制限開始位置又は制限開始速度の少なくともいずれかに達するかもしくはこれを超えたときに、コントローラ２は、既に述べた本発明の実施例の手法によって、可動域制限トルク又は可動域制限力を発生させる。このような特徴により、本発明によれば、操作者は、ロボット１が現在可動域限界に近づきつつあることを認識することができ、ロボットの機械損傷を回避することができる。軸や手先の位置だけでなく速度にも基づいて可動域制限トルクや可動域制限力の計算を行うことにより、柔軟制御中にロボットに対して急激に大きな外力が働いた場合に、ロボット各軸や手先の位置の急激な変化に適切に対応し得る可動域制限を行うことができる。
【００６８】
直接教示は、ロボットの手先部等を直接操作して所望する位置に導き、位置登録するという過程で行われる。このような直接教示においては、操作者がロボットの動作位置に注視していなかったためにロボットが可動域限界に達してしまい、ロボットに機械損傷を与えたり、もしくは、ロボットを周辺装置と衝突させたりするという問題が起こり得る。本発明の実施例によれば、これらの問題を回避できる。すなわち、ロボットの動作領域を予め設定しておくことで、操作者の教示作業においてロボットが動作限界や周辺装置の付近に近付いた時に操作者に反力を提示することにより、上述のような機械損傷や衝突を防ぐことが出来るのである。
【００６９】
また、予め複数の周辺機器の配置情報を基にロボットの柔軟制御動作時の可動領域を必要最小限に制限しておくことで、ロボットがある周辺機器から外力を受けて想定以上に倣いそうになった場合でも、ロボットと別の周辺機器との干渉を回避できる。
【００７０】
また、本実施例では作業座標系の直交成分のみの説明であったが、回転成分でも同様の設定が可能であることは明白である。
【符号の説明】
【００７１】
１ ロボット
２ コントローラ
３ 教示装置
４ 操作者
１０１ａ〜ｆ ロボットの関節軸（６軸）
２０１ａ〜ｆ 柔軟制御状態での可動域制限付き位置制御部（６軸）
２０２ａ 関節座標系における可動域制限トルク計算部
２０３ａ トルク制限部
２０４、２０４ａ 重力補償トルク計算部
２０５、２０５ａ 摩擦補償トルク計算部
２０６ａ 駆動モータ
２０７ａ 速度演算部
２０８ａ 加算部
２０９ａ 位置フィードバックゲイン要素
２１０ａ 加算部
２１１ａ 速度フィードバックゲイン要素
２１２ 作業座標系における可動域制限力計算部
２１３ 手先力制限部
２１４ 逆計算処理部
２１５ 順運動学計算部
２１６ 加算部
２１７ 位置フィードバックゲイン要素
２１８ 加算部
２１９ 速度フィードバックゲイン要素
２２０ 速度演算部
２２１ 順運動学計算部
２２２ 加算部
【特許請求の範囲】
【請求項１】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御装置であって、各関節につき、
関節位置指令に対して、該関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、
該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算する制限トルク計算手段であって、前記制限トルクは、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限トルク計算手段と
を備え、
前記速度フィードバック制御手段の出力に前記計算された制限トルクを加算して得られるトルクにより該関節の動作を制御する制御装置。
【請求項２】
前記制限トルクは、前記制限開始位置と前記現在位置との差に基づいて計算される第１の制限トルクと前記制限開始速度と前記現在速度との差に基づいて計算される第２の制限トルクとの和である請求項１に記載の制御装置。
【請求項３】
前記制限トルク（ＴＬｎｋ）は、以下の式
【数１】
により計算され、ここで、ＴＬｎｋ_θ及びＴＬｎｋ_ωはそれぞれ前記第１の制限トルク及び前記第２の制限トルク、ｋＰθ及びｋＭθはそれぞれ正負方向の位置に関する制限トルク計算定数、ｋＰω及びｋＭωはそれぞれ正負方向の速度に関する制限トルク計算定数、θ及びωはそれぞれ現在位置及び現在速度、θＰＬｉｍ及びθＭＬｉｍはそれぞれ正負方向の制限開始位置、θＰＴｒｍ及びθＭＴｒｍはそれぞれ正負方向の限界位置、ωＰＬｉｍ及びωＭＬｉｍはそれぞれ正負方向の制限開始速度、ωＰＴｒｍ及びωＭＴｒｍはそれぞれ正負方向の限界速度である請求項２に記載の制御装置。
【請求項４】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御装置であって、
各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定する順運動学計算部と、
前記手先位置指令に対して、各関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、
作業座標系において、前記手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と前記手先の現在位置との関係、及び前記手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と前記手先の現在速度との関係に基づいて、制限力を計算する制限力計算手段であって、前記制限力は、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限力計算手段と、
前記速度フィードバック制御手段の出力に前記計算された制限力を加算して得られる力から各関節に要求されるトルクを計算する逆計算処理手段と
を備え、
前記計算されたトルクにより各関節の動作を制御する制御装置。
【請求項５】
前記制限力は、前記制限開始位置と前記現在位置との差に基づいて計算される第１の制限力と前記制限開始速度と前記現在速度との差に基づいて計算される第２の制限力との和である請求項４に記載の制御装置。
【請求項６】
前記制限力（ＦＬｎｒ）は、以下の式
【数２】
により計算され、ここで、ＦＬｎｒ_ｘ及びＦＬｎｒ_ｖはそれぞれ前記第１の制限力及び前記第２の制限力、ｋＰｘ及びｋＭｘはそれぞれ正負方向の位置に関する制限力計算定数、ｋＰｖ及びｋＭｖはそれぞれ正負方向の速度に関する制限力計算定数、ｘ及びｖはそれぞれ現在位置及び現在速度、ｘＰＬｉｍ及びｘＭＬｉｍはそれぞれ正負方向の制限開始位置、ｘＰＴｒｍ及びｘＭＴｒｍはそれぞれ正負方向の限界位置、ｖＰＬｉｍ及びｖＭＬｉｍはそれぞれ正負方向の制限開始速度、ｖＰＴｒｍ及びｖＭＴｒｍはそれぞれ正負方向の限界速度である請求項５に記載の制御装置。
【請求項７】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御方法であって、少なくとも１つの関節につき、
関節位置指令に対して、該関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、
該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算するステップであって、前記制限トルクは、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限トルクを計算するステップと、
前記速度フィードバック制御の出力に前記計算された制限トルクを加算して得られるト
ルクにより該関節の動作を制御するステップと
を備える制御方法。
【請求項８】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御方法であって、
順運動学計算により、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定するステップと、
前記手先位置指令に対して、各関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、
作業座標系において、前記手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と前記手先の現在位置との関係、及び前記手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と前記手先の現在速度との関係に基づいて、制限力を計算するステップであって、前記制限力は、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限力を計算するステップと、
逆計算により、前記速度フィードバック制御の出力に前記計算された制限力を加算して得られる力から各関節に要求されるトルクを計算するステップと、
前記計算されたトルクにより各関節の動作を制御するステップと
を備える制御方法。
【請求項１】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御装置であって、各関節につき、
関節位置指令に対して、該関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、
該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算する制限トルク計算手段であって、前記制限トルクは、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限トルク計算手段と
を備え、
前記速度フィードバック制御手段の出力に前記計算された制限トルクを加算して得られるトルクにより該関節の動作を制御する制御装置。
【請求項２】
前記制限トルクは、前記制限開始位置と前記現在位置との差に基づいて計算される第１の制限トルクと前記制限開始速度と前記現在速度との差に基づいて計算される第２の制限トルクとの和である請求項１に記載の制御装置。
【請求項３】
前記制限トルク（ＴＬｎｋ）は、以下の式
【数１】
により計算され、ここで、ＴＬｎｋ_θ及びＴＬｎｋ_ωはそれぞれ前記第１の制限トルク及び前記第２の制限トルク、ｋＰθ及びｋＭθはそれぞれ正負方向の位置に関する制限トルク計算定数、ｋＰω及びｋＭωはそれぞれ正負方向の速度に関する制限トルク計算定数、θ及びωはそれぞれ現在位置及び現在速度、θＰＬｉｍ及びθＭＬｉｍはそれぞれ正負方向の制限開始位置、θＰＴｒｍ及びθＭＴｒｍはそれぞれ正負方向の限界位置、ωＰＬｉｍ及びωＭＬｉｍはそれぞれ正負方向の制限開始速度、ωＰＴｒｍ及びωＭＴｒｍはそれぞれ正負方向の限界速度である請求項２に記載の制御装置。
【請求項４】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御装置であって、
各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定する順運動学計算部と、
前記手先位置指令に対して、各関節の位置に基づいてそれぞれ位置フィードバック制御及び速度フィードバック制御を行う、位置フィードバック制御手段及び速度フィードバック制御手段と、
作業座標系において、前記手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と前記手先の現在位置との関係、及び前記手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と前記手先の現在速度との関係に基づいて、制限力を計算する制限力計算手段であって、前記制限力は、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限力計算手段と、
前記速度フィードバック制御手段の出力に前記計算された制限力を加算して得られる力から各関節に要求されるトルクを計算する逆計算処理手段と
を備え、
前記計算されたトルクにより各関節の動作を制御する制御装置。
【請求項５】
前記制限力は、前記制限開始位置と前記現在位置との差に基づいて計算される第１の制限力と前記制限開始速度と前記現在速度との差に基づいて計算される第２の制限力との和である請求項４に記載の制御装置。
【請求項６】
前記制限力（ＦＬｎｒ）は、以下の式
【数２】
により計算され、ここで、ＦＬｎｒ_ｘ及びＦＬｎｒ_ｖはそれぞれ前記第１の制限力及び前記第２の制限力、ｋＰｘ及びｋＭｘはそれぞれ正負方向の位置に関する制限力計算定数、ｋＰｖ及びｋＭｖはそれぞれ正負方向の速度に関する制限力計算定数、ｘ及びｖはそれぞれ現在位置及び現在速度、ｘＰＬｉｍ及びｘＭＬｉｍはそれぞれ正負方向の制限開始位置、ｘＰＴｒｍ及びｘＭＴｒｍはそれぞれ正負方向の限界位置、ｖＰＬｉｍ及びｖＭＬｉｍはそれぞれ正負方向の制限開始速度、ｖＰＴｒｍ及びｖＭＴｒｍはそれぞれ正負方向の限界速度である請求項５に記載の制御装置。
【請求項７】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御方法であって、少なくとも１つの関節につき、
関節位置指令に対して、該関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、
該関節に設定された関節座標系において、該関節につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と該関節の現在位置との関係、及び該関節につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と該関節の現在速度との関係に基づいて、制限トルクを計算するステップであって、前記制限トルクは、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限トルクを計算するステップと、
前記速度フィードバック制御の出力に前記計算された制限トルクを加算して得られるト
ルクにより該関節の動作を制御するステップと
を備える制御方法。
【請求項８】
少なくとも１つの関節を有するロボットの柔軟制御に使用する制御方法であって、
順運動学計算により、各関節に対する関節位置指令から作業座標系におけるロボットの手先位置指令を決定するステップと、
前記手先位置指令に対して、各関節の位置に基づいて位置フィードバック制御及び速度フィードバック制御を行うステップと、
作業座標系において、前記手先につき予め設定された限界位置よりも絶対値が所定量だけ小さい制限開始位置と前記手先の現在位置との関係、及び前記手先につき予め設定された限界速度よりも絶対値が所定量だけ小さい制限開始速度と前記手先の現在速度との関係に基づいて、制限力を計算するステップであって、前記制限力は、前記現在位置が前記制限開始位置を越えて大きくなるにつれて又は前記現在速度が前記制限開始速度を超えて大きくなるにつれて大きくなる、制限力を計算するステップと、
逆計算により、前記速度フィードバック制御の出力に前記計算された制限力を加算して得られる力から各関節に要求されるトルクを計算するステップと、
前記計算されたトルクにより各関節の動作を制御するステップと
を備える制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−２０６８８６（Ｐ２０１１−２０６８８６Ａ）
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願番号】特願２０１０−７７３６８（Ｐ２０１０−７７３６８）
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２３年１０月２０日（２０１１．１０．２０）
【国際特許分類】
【出願日】平成２２年３月３０日（２０１０．３．３０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]