(57)【要約】【課題】計算量を大幅に低減してＣＰＵの負担を軽減することのできるロボットの制御装置を提供する。【解決手段】互いに干渉する複数軸から構成されるロボットであって、モータ８，１８と、モータ８，１８に減速機等を介して結合されたアーム９，１９と、モータ位置検出器１０，２０とから構成され、各軸を各軸毎の指令通りに動作させるための位置制御部１，１１および速度制御部２，１２を備えたロボットの制御装置において、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部７，１７と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を求める非干渉トルク信号作成部６，１６と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を求める非干渉位置信号作成部５，１５とを備えた。【選択図】  図１
2005186235.tif 000002
【特許請求の範囲】
【請求項１】
互いに干渉する複数軸から構成されるロボットであって、モータと、前記モータに減速機等を介して結合されたアームと、前記モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置において、
自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、
他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、
他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉位置信号作成部と
を備えたことを特徴とするロボットの制御装置。
【請求項２】
軸が２軸である場合の干渉について、
前記干渉力計算部における干渉力は、次式
ｄis1＝Ｂ＊ｘref1＊ｓ2
ｄis2＝Ａ＊ｘref2＊ｓ2
によって計算し、
前記非干渉トルク信号作成部における非干渉トルク信号は、次式
ｔref_ff1＝（Ｊm1＊ＪL1／Ｋ1＊ｓ4＋（Ｊm1＋ＪL1）ｓ2）＊ｘref1
−（Ｊm1／Ｋ1＊ｓ2＋１）＊ｄis2
ｔref_ff2＝（Ｊm2＊ＪL2／Ｋ2＊ｓ4＋（Ｊm2＋ＪL2）ｓ2）＊ｘref2
−（Ｊm2／Ｋ2＊ｓ2＋１）＊ｄis1
によって計算し、
前記非干渉位置信号作成部における非干渉位置信号は、次式
ｘref_ff1＝（ＪL1／Ｋ1＊ｓ2＋１）＊ｘref1−１／Ｋ1＊ｄis2
ｘref_ff2＝（ＪL2／Ｋ2＊ｓ2＋１）＊ｘref2−１／Ｋ2＊ｄis1
によって計算することを特徴とする請求項１記載のロボットの制御装置。
但し、Ｊm1：１軸目モータ慣性モーメント
ＪL1：１軸目アーム慣性モーメント
Ｋ1：１軸目減速機ばね定数
Ｊm2：２軸目モータ慣性モーメント
ＪL2：２軸目アーム慣性モーメント
Ｋ2：２軸目減速機ばね定数
Ａ（＝Ｂ）：２軸の構成、成す角度、幾何学的関係から求まる係数
ｓ：ラプラス演算子
ｄis1：１軸目から２軸目に作用する干渉力
ｄis2：２軸目から１軸目に作用する干渉力
ｘref1：１軸目位置指令
ｘref2：２軸目位置指令
【請求項３】
前記、非干渉トルク信号作成部および非干渉位置信号作成部の処理で使用する、他軸から作用する干渉力の計算値は、軸が２軸を超える場合、各軸から自軸へ作用する干渉力計算値の総和であることを特徴とする請求項１記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、互いに干渉する複数軸で構成されるロボットを、干渉力が作用しても各軸が指令どおり動作するように、制御を行うロボットの制御装置に関する。
【背景技術】
【０００２】
従来のロボット制御装置は、モデル制御器を有し、モデル制御器内部では、擬似モデルと、擬似モデル用の制御部と、干渉トルクを相殺するためにモデル補償トルクを算出する補正量算出部とを有し、モデル補償トルクが加算されたモデルフィードフォワード指令を、フィードフォワード信号として出力することで、非干渉化を行っている（例えば、特許文献１参照）。
図５は特許文献１に開示された従来の実施の一形態のロボット制御装置の構成を示す制御ブロック図であり、図６は、図５のモデル制御器の構成を示す制御ブロック図である。図５では、各軸ごとに２慣性系（電動機＋減速機等のバネ＋アーム）に近似した２軸のロボット制御系の基本構成を示している。図５および図６では、２軸をＬ軸、Ｕ軸とし、Ｌ軸に関する指令や量についてはそのサフィックスに“＿Ｌ”付加し、Ｕ軸に関する指令や量についてはそのサフィックスに“＿Ｕ”を付加している。
【０００３】
このロボット制御装置は、各軸ごとの電動機位置指令Ｘref_L，Ｘref_Uに基づいてＬ軸及びＵ軸の制御を行うものであり、２軸のフィードバック制御系（フィードバック制御器）５３Ｌ，５３Ｕと制御対象（電動機、減速機、ロボットアーム）との擬似モデルであるモデル制御器５１を有している。モデル制御器５１は、電動機位置指令Ｘref_L，Ｘref_Uを入力とし、ロボットのダイナミクスを考慮して各軸のフィードフォワード指令ＵFF_L，ＵFF_Uを計算し、各軸の電動機の加速度項へのフィードフォワード補償を行うとともに、規範となるモデルにおける各軸の電動機位置θMm_L，θMm_U、電動機速度
【数１】
2005186235.tif 000003
ねじれ角θMs_L，θMs_U、ねじれ角速度
【数２】
2005186235.tif 000004
を算出して出力する。
【０００４】
以下、モデル制御器５１の出力である各要素には、接頭語「モデル」を付加する。
一方、フィードバック制御系（フィードバック制御器）５３Ｌ，５３Ｕにおいて、各軸の位置ゲインはＫp_L，Ｋp_U、速度ゲインはＫv_L，Ｋv_U、トルク定数はＫt_L，Ｋt_Uであり、電動機に与えられる最終的な加速度指令はそれぞれＵref_L，Ｕref_Uである。また、各軸の電動機の慣性モーメントはそれぞれＪm_L，Ｊm_L、電流指令はＩref_L，Ｉref_Uである。電動機の実際の位置はθm_L，θm_Uであり、電動機の実際の速度は
【数３】
2005186235.tif 000005
であり、電動機の実際の加速度は
【数４】
2005186235.tif 000006
である。
さらに減速機での減速比がＮ_L，Ｎ_U、減速機のバネ定数がＫc_L，Ｋc_U、アームの慣性モーメントがＪL_L，ＪL_U、アームの実際の位置がθL_L，θL_U、アームの加速度（負荷加速度）が
【数５】
2005186235.tif 000007
で表わされている。そして、両軸間の干渉を表わすために、ロボットアームの質量や両軸間の角度により決定されるＭＬＵ／Ｍｏ２の値がＬ軸の負荷トルクに乗じられてＵ軸の負荷加速度に作用し、同様に、ＭＵＬ／Ｍｏ２の値がＵ軸の負荷トルクに乗じられてＬ軸の負荷加速度に作用する構成となっている。
【０００５】
このロボット制御系には、Ｌ軸、Ｕ軸のそれぞれごとに、電動機への最終的な加速度指令Ｕref_L，Ｕref_Uと電動機の実際の位置θm_L，θm_Uを入力として、実際のねじれ角θs_L，θs_Uとその角速度
【数６】
2005186235.tif 000008
とを推定する状態観測器５２Ｌ，５２Ｕが設けられている。さて、このフィードバック制御器では、モデル制御器５１から出力されるモデル電動機位置θMm_L，θMm_Uと実際の電動機位置θm_L，θm_Uとの偏差に対して位置ゲインＫp_L，Ｋp_Uを乗算して、それぞれの軸の速度指令としている。
このように得られた各速度指令に、モデル電動機速度
【数７】
2005186235.tif 000009
と実際の電動機速度
【数８】
2005186235.tif 000010
との偏差を加算し、速度ゲインＫv_L，Ｋv_Uを乗算して、それぞれ、加速度指令としている。
【０００６】
このような加速度指令に対して、（ａ）モデル制御器５１からのフィードフォワード指令ＵFF_L，ＵFF_Uと、（ｂ）モデルねじれ角θMs_L，θMs_Uと状態観測器５２Ｌ，５２Ｕから出力されるねじれ角θs_L，θs_Uとの偏差にフィードバックゲインＫ1_L，Ｋ1_Uを乗じたものと、（ｃ）モデルねじれ角速度
【数９】
2005186235.tif 000011
と状態観測器５２Ｌ，５２Ｕから出力されるねじれ角速度
【数１０】
2005186235.tif 000012
との偏差にフィードバックゲインＫ2_L，Ｋ2_Uを乗じたものとを加算し、それぞれ、各軸に対する最終的な電動機加速度指令Ｕref_L，Ｕref_Uとしている。
このロボット制御系では、このように得られた最終的な電動機加速度指令Ｕref_L，Ｕref_Uにより各軸の電動機が駆動され、減速比がそれぞれＮ_L，Ｎ_Uである減速機を介して、各軸のアームが駆動される。その際、上述したように、軸間の干渉力が各軸の負荷加速度に作用する。
【０００７】
次に、モデル制御器５１の構成を説明する。図６は、フィードバック制御系や電動機、減速機、ロボットアームを表す擬似モデルからなるモデル制御器５１の構成の詳細を示している。擬似モデルにおける各軸の位置ゲインと速度ゲインがそれぞれモデル位置ゲインＫpM_L，ＫpM_Uとモデル速度ゲインＫvM_L，ＫvM_Uである。同様に、モデル電動機慣性モーメントＪmM_L，ＪmM_U、モデルアーム慣性モーメントＪLM_L，ＪLM_U、モデル減速比ＮM_L，ＮM_U、モデル減速機バネ定数ＫcM_L，ＫcM_Uが、モデルパラメータとして定められている。
モデル制御器５１は、各軸の電動機位置指令Ｘref_L，Ｘref_Uを入力とし、この電動機位置指令Ｘref_L，Ｘref_Uとモデル電動機位置θMm_L，θMm_Uとの偏差にモデル位置ゲインＫpM_L，ＫpM_Uを乗じて各軸のモデル速度指令としている。そして、これらのモデル速度指令から各軸のモデル電動機速度
【数１１】
2005186235.tif 000013
を減算した値にモデル速度ゲインＫvM_L，ＫvM_Uを乗算して、それぞれ各軸のモデル加速度指令としている。
【０００８】
このモデル加速度指令に、モデル減速機から得られるモデルねじれ角θMs_L，θMs_UにモデルフィードバックゲインＫ1M_L，Ｋ1M_Uを乗じて得られる値と、モデルねじれ角θMs_L，θMs_Uを微分して得られるモデルねじれ角速度
【数１２】
2005186235.tif 000014
にモデルフィードバックゲインＫ2M_L，Ｋ2M_Uを乗じて得られる値とを減算し、モデル加速度指令ＵMref_L，ＵMref_Lとしている。このモデル加速度指令ＵMref_L，ＵMref_Uが、モデル電動機に入力されている。
したがって、モデル加速度指令ＵMref_L，ＵMref_Uは、
【数１３】
2005186235.tif 000015
と表わすことができる。
しかし、ここで想定している２軸ロボットでは、干渉により、Ｌ軸の負荷トルクに、ロボットアームの質量や両軸間の角度により決定される値ＭLUM／ＭoM２が乗じられて、Ｕ軸の負荷加速度に作用する。同様に、Ｕ軸の負荷トルクに、値ＭULM／ＭoM２が乗じられて、Ｌ軸の負荷加速度に作用する。
【０００９】
このような干渉をアーム側に作用する外乱としてとらえると、Ｌ軸に関しては、下式に示すモデル補正トルクＴcomp_Lをモデル電動機加速度指令ＵMref_Lに加算すれば、干渉によるアームの振動を軽減することができる。
【数１４】
2005186235.tif 000016
ここで、Ｄis_ULは、Ｕ軸からＬ軸に作用する干渉トルクであって、
【数１５】
2005186235.tif 000017
と表わすことができる。よって、最終的なモデル電動機加速度指令ＵFF_Lを
ＵFF_L＝ＵMref_L＋Ｔcomp_L（５）
として、電動機の加速度項へフィードフォワード補償を行う。このモデル制御器５１には、モデルねじれ角θMs_Uを入力として上述の式（３），（４）に基づいてモデル補正トルクＴcomp_Lを算出する補正量演算部５４Ｌが設けられており、補正量演算部５４Ｌからのモデル補正トルクＴcomp_Lがモデル電動機加速度指令ＵMref_Lに加算される。
【００１０】
同様に、Ｕ軸についても、モデル補正トルクＴcomp_Uを
【数１６】
2005186235.tif 000018
のように定める。ここでＤis_LUは、Ｌ軸からＵ軸に作用する干渉トルクであって、
【数１７】
2005186235.tif 000019
である。モデルねじれ角θMs_Lを入力として式（６），（７）に基づきモデル補正トルクＴcomp_Uを求める補正量演算部５４Ｕが設けられている。その結果、最終的なモデル電動機加速度指令ＵFF_Uは、
ＵFF_U＝ＵMref_U＋Ｔcomp_U        （８）
となる。
【００１１】
以上のように、特許文献１に開示された従来技術においては、このようにモデル制御器５１により干渉トルク補正を行った上で、規範となる各状態量を指令としてフィードバック制御系に入力するので、各ロボットアームは、他軸からの干渉による影響を受けなくなる。
このように、従来のロボット制御装置は、他軸から受ける干渉力をモデル制御器で補正し、それを基に規範となる各状態量をフィードバック制御器に入力することにより、補正トルクにノイズ成分が重畳することがなくなり、ロボットアームは干渉による振動を受けずにツール先端の軌跡精度を向上させるのである。
【００１２】
【特許文献１】特開平１０−３２９０６３号公報（第２−５頁、図１、図２）
【発明の開示】
【発明が解決しようとする課題】
【００１３】
しかしながら、従来のロボット制御装置は、この制御器を実現するために、フィードバック制御部の演算以外に、モデル制御器５１内で、制御周期毎に、ねじれ角の計算など擬似モデルの計算を行う必要があり各軸毎の計算量が膨大になるという問題があった。
また、モデル制御器５１内で、フィードバック制御部の演算以外に、制御周期毎に、擬似モデルを制御するためのモデル用のフィードバック制御の計算を行う必要があり各軸毎の計算量がさらに増えるという問題がある。
また、非干渉化をトルクの補正のみで行うため、モデル補正トルクＴcompの計算にモデル位置ゲインＫpMやモデル速度ゲインＫvMの値まで必要となり、さらに計算が複雑になるという問題もある。
そして、結果として、各軸毎の計算量が膨大になり、制御演算時間が長くなり、本制御装置を実現するために高速なＣＰＵを必要とするという問題がある。
本発明はこのような問題点に鑑みてなされたものであり、計算量を大幅に低減してＣＰＵの負担を軽減することのできるロボットの制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
上記問題を解決するため、本発明の第１の構成は、互いに干渉する複数軸から構成されるロボットであって、モータと、前記モータに減速機等を介して結合されたアームと、前記モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置において、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉位置信号作成部とを備えたことを特徴とするものである。
この第１の構成においては、干渉力計算部と非干渉トルク信号作成部と非干渉位置信号作成部を有し、擬似モデルの計算と擬似モデル用の制御の計算は行わず、且つ、モータトルク信号だけではなく、モータ位置信号を干渉を考慮して作成することで、計算量が大幅に低減する。
【００１５】
本発明の第２の構成は、軸が２軸である場合の干渉について、前記干渉力計算部における干渉力は、次式
ｄis1＝Ｂ＊ｘref1＊ｓ2
ｄis2＝Ａ＊ｘref2＊ｓ2
によって計算し、
前記非干渉トルク信号作成部における非干渉トルク信号は、次式
ｔref_ff1＝（Ｊm1＊ＪL1／Ｋ1＊ｓ4＋（Ｊm1＋ＪL1）ｓ2）＊ｘref1
−（Ｊm1／Ｋ1＊ｓ2＋１）＊ｄis2
ｔref_ff2＝（Ｊm2＊ＪL2／Ｋ2＊ｓ4＋（Ｊm2＋ＪL2）ｓ2）＊ｘref2
−（Ｊm2／Ｋ2＊ｓ2＋１）＊ｄis1
によって計算し、
前記非干渉位置信号作成部における非干渉位置信号は、次式
ｘref_ff1＝（ＪL1／Ｋ1＊ｓ2＋１）＊ｘref1−１／Ｋ1＊ｄis2
ｘref_ff2＝（ＪL2／Ｋ2＊ｓ2＋１）＊ｘref2−１／Ｋ2＊ｄis1
によって計算することを特徴とする。
但し、Ｊm1：１軸目モータ慣性モーメント
ＪL1：１軸目アーム慣性モーメント
Ｋ1：１軸目減速機ばね定数
Ｊm2：２軸目モータ慣性モーメント
ＪL2：２軸目アーム慣性モーメント
Ｋ2：２軸目減速機ばね定数
Ａ（＝Ｂ）：２軸の構成、成す角度、幾何学的関係から求まる係数
ｓ：ラプラス演算子
ｄis1：１軸目から２軸目に作用する干渉力
ｄis2：２軸目から１軸目に作用する干渉力
ｘref1：１軸目位置指令
ｘref2：２軸目位置指令
この第２の構成においては、干渉力、非干渉トルク信号、および非干渉位置信号を計算する数式が特定される。
【００１６】
また、本発明の第３の構成は、前記、非干渉トルク信号作成部および非干渉位置信号作成部の処理で使用する、他軸から作用する干渉力の計算値は、軸が２軸を超える場合、各軸から自軸へ作用する干渉力計算値の総和であることを特徴とするものである。
この第３の構成においては、複数軸間に干渉力が作用する場合に、自軸に対しても複数の軸から干渉力を受ける。その場合、２軸間の干渉力の総和を計算することにより、自軸に作用する干渉力を求めることができる。
【発明の効果】
【００１７】
本発明の第１の構成によれば、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようにモータを動作させることができ、結果として干渉の影響を受けないで、指令どおりロボットアームを動作させることができる。また、非干渉化を実現するための計算を非常に少なくできる。その結果、ＣＰＵの計算量を大幅に低減することができるため、安価なＣＰＵを用いることができる。
本発明の第２の構成によれば、干渉力、非干渉トルク信号、および非干渉位置信号を計算する数式を特定することにより、ロボット制御を具体化することができる。
また、第３の構成によれば、自軸に対し複数の軸からの干渉力が作用する場合も、複数軸の関係を考慮して複雑な計算をすることなく、各軸から自軸へ作用する干渉力の合計値を計算する処理を行うだけで、後は２軸間の非干渉化と同様の処理を行うのみでよく、簡単に複数軸間の非干渉化が可能になる。
また、考慮していなかった軸間の干渉を追加する場合などもプログラム修正はほとんどなく、各軸から作用する干渉力の合計値を計算する処理を追加するだけで簡単に全ての軸の非干渉化を実現することができる。
【発明を実施するための最良の形態】
【００１８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１９】
図１は、本発明のロボットの制御装置を説明する全体構成図である。図において、１００は１軸目ロボットの制御装置、２００は２軸目ロボットの制御装置を表している。
１軸目ロボットの制御装置１００は、１軸目位置指令ｘref1と１軸目モータ位置検出値ｘfb1と２軸目から１軸目に作用する干渉力計算値ｄis2を入力し、制御対象が指令通り動作するよう制御演算を行い、１軸目トルク指令ｔref1を出力する。８は１軸目モータを表し１軸目ロボットの制御装置１００から出力される１軸目トルク指令ｔref1により動作する。９は１軸目アームであり、モータ８と減速機等を介して結合されている。１０は１軸目モータ８の位置ｘfb1を検出する１軸目モータ位置検出器である。
２軸目ロボットの制御装置２００は、２軸目位置指令ｘref2と２軸目モータ位置検出値ｘfb2と１軸目から２軸目に作用する干渉力計算値ｄis1を入力し、制御対象が指令通り動作するよう制御演算を行い、２軸目トルク指令ｔref2を出力する。１８は２軸目モータを表し２軸目ロボットの制御装置２００から出力される２軸目トルク指令ｔref2により動作する。１９は２軸目アームであり、２軸目モータ１８と減速機等を介して結合されている。２０は２軸目モータ１８の位置ｘfb2を検出する２軸目モータ位置検出器である。
また、両軸とも、通常、出力されたトルク指令値ｔrefは電流指令値に変換され電流アンプを経てモータが動作するが、その応答は無視できるほど十分に速く、また本発明では特に関係がないため省略している。
【００２０】
ここで、モータ８，１８とアーム９，１９を結合する減速機は、ばね要素を有するため、ロボットの制御装置１００およびロボットの制御装置２００の制御対象は、どちらも、モータ慣性モーメントとアーム慣性モーメントが、ばね要素で結合された２慣性系と考えられる。
本実施例では、単軸ごとの制御対象を２慣性系とし、互いの軸間の干渉を考えた図４に示す伝達関数で表される構成を考える。
図４中、各記号の意味は以下の通りである。
Ｊm1：１軸目モータ慣性モーメント
ＪL1：１軸目アーム慣性モーメント
Ｋ1：１軸目減速機ばね定数
Ｊm2：２軸目モータ慣性モーメント
ＪL2：２軸目アーム慣性モーメント
Ｋ2：２軸目減速機ばね定数
Ａ，Ｂ：２軸の構成、成す角度、幾何学的関係から求まる係数
ｓ：ラプラス演算子
ｄis1：１軸目から２軸目に作用する干渉力
ｄis2：２軸目から１軸目に作用する干渉力
ｘ１：アーム１の位置
ｘ２：アーム２の位置
また、上記Ａ，Ｂは力学計算で求めることが可能である。
例えば２つの軸が図３に示すような関係の場合を考える。１軸目と２軸目の成す角度をθとし、１軸目と２軸目の軸間の距離をＬで表し、各軸、回転中心から質量中心までの距離をＬ1、Ｌ2で表し、それぞれの質量をＭ1，Ｍ2とすると、Ａ，Ｂは式（９）のようになる。
Ａ＝Ｂ＝Ｍ2＊Ｌ2＊Ｌ＊ｃｏｓ（θ）＋Ｍ2＊Ｌ22・・・（９）
【００２１】
以下、実際に１軸目ロボットの制御装置１００内部の構成を説明する。
図１において、１は１軸目の位置制御部であり、１軸目非干渉位置信号ｘref_ff1と１軸目位置検出値ｘfb1が一致するよう、それらの差である１軸目位置偏差ｅｒｒ１を入力し、制御演算を行い、１軸目速度指令ｖref1を出力する。
位置制御部１内の処理はどのように行っても良いが、例えば式（１０）に示すように１軸目位置ループ比例ゲインＫp1を用いて比例制御を行えばよい。
ｖref1＝Ｋp1＊ｅｒｒ１                  ・・・（１０）
２は１軸目速度制御部であり、１軸目非干渉速度信号ｖref_ff1と１軸目の速度検出値ｖfb1が一致するように、式（１１）で計算される１軸目速度偏差ｖerr1を入力し、制御演算を行い、１軸目トルク指令値ｔref_fb1を出力する。
ｖerr1=ｖref_ff1＋ｖref1−ｖfb1    ・・・（１１）
【００２２】
速度制御部２内の処理はどのように行っても良いが、例えば式（１２）に示すように１軸目速度ループ比例ゲインＫv1および１軸目速度ループ積分ゲインＫi1を用いて比例積分制御を行えばよい。
ｔref_fb1＝Ｋv1＊（１＋Ｋi1／ｓ）＊ｖerr1・・・（１２）
３と４は微分器を表す。
５は１軸目非干渉位置信号作成部であり、１軸目位置指令ｘref1と２軸目から１軸目へ作用する干渉力計算値ｄis2を入力し、干渉力ｄis2が作用しても、１軸目位置指令ｘref1通りにアーム９先端ｘ１が動作するための１軸目モータ位置信号を図４に示す２軸の干渉モデルの関係を用いて作成し、１軸目非干渉位置信号ｘref_ff1として出力する。ｘref_ff1の計算式を式（１３）に示す。
ｘref_ff1＝（ＪL1／Ｋ1＊ｓ2＋１）＊ｘref1−１／Ｋ1＊ｄis2      ・・・（１３）
【００２３】
６は１軸目非干渉トルク信号作成部であり、１軸目位置指令ｘref1と２軸目から１軸目へ作用する干渉力計算値ｄis2を入力し、干渉力ｄis2の影響が作用しても、１軸目位置指令ｘref1通りにアーム９先端ｘ１が動作するための１軸目モータトルク指令信号を図４に示す２軸の干渉モデルの関係を用いて作成し、１軸目非干渉トルク信号ｔref_ff1として出力する。ｔref_ff1の計算式を式（１４）に示す。
ｔref_ff1＝（Ｊm1＊ＪL1／Ｋ1＊ｓ4＋（Ｊm1＋ＪL1）ｓ2）＊ｘref1
−（Ｊm1／Ｋ1＊ｓ2＋１）＊ｄis2・・・（１４）
７は１軸目干渉力計算部であり、１軸目位置指令ｘref1を入力し、１軸目から２軸目へ作用する干渉力ｄis1を計算し出力する。図４から分かるように、ｄis1は式（１５）で計算される。
ｄis1＝Ｂ＊ｘref1＊ｓ2・・・（１５）
最終的に１軸目ロボットの制御装置１００からモータ８ヘ出力される１軸目トルク指令値ｔref1は以下の式（１６）で計算される。
ｔref1＝ｔref_ff1＋ｔref_fb1・・・（１６）
【００２４】
次に、２軸目ロボットの制御装置２００内部の構成を説明する。
図において、１１は２軸目の位置制御部であり、２軸目非干渉位置信号ｘref_ff2と２軸目位置検出値ｘfb2が一致するよう、それらの差である２軸目位置偏差ｅｒｒ２を入力し、制御演算を行い、２軸目速度指令ｖref2を出力する。
位置制御部１１内の処理はどのように行っても良いが、例えば式（１７）に示すように２軸目位置ループ比例ゲインＫp2を用いて比例制御を行えばよい。
ｖref2＝Ｋp2＊ｅｒｒ２                  ・・・（１７）
１２は２軸目速度制御部であり、２軸目非干渉速度信号ｖref_ff2と２軸目の速度検出値ｖfb2が一致するように、式（１８）で計算される２軸目速度偏差ｖerr2を入力し、制御演算を行い、２軸目トルク指令値ｔref_fb2を出力する。
ｖerr2＝ｖref_ff2＋ｖref2ｖfb2・・・（１８）
【００２５】
速度制御部１２内の処理はどのように行っても良いが、例えば式（１９）に示すように２軸目速度ループ比例ゲインＫv2および２軸目速度ループ積分ゲインＫi2を用いて比例積分制御を行えばよい。
ｔref_fb2＝Ｋv2＊（１＋Ｋi2／ｓ）＊ｖerr2・・・（１９）
１３と１４は微分器を表す。
１５は２軸目非干渉位置信号作成部であり、２軸目位置指令ｘref2と１軸目から２軸目へ作用する干渉力計算値ｄis1を入力し、干渉力ｄis1が作用しても、２軸目位置指令ｘref2通りにアーム１９先端ｘ２が動作するための２軸目モータ位置信号を図４に示す２軸の干渉モデルの関係を用いて作成し、２軸目非干渉位置信号ｘref_ff2として出力する。ｘref_ff2の計算式を式（２０）に示す。
ｘref_ff2＝（ＪL2／Ｋ2＊ｓ2＋１）＊ｘref2−１／Ｋ2＊ｄis1      ・・・（２０）
【００２６】
１６は２軸目非干渉トルク信号作成部であり、２軸目位置指令ｘref2と１軸目から２軸目へ作用する干渉力計算値ｄis1を入力し、干渉力ｄis1の影響が作用しても、２軸目位置指令ｘref2通りにアーム１９先端ｘ２が動作するための２軸目モータトルク指令信号を図４に示す２軸の干渉モデルの関係を用いて作成し、２軸目非干渉トルク信号ｔref_ff2として出力する。ｔref_ff2の計算式を式（２１）に示す。
ｔref_ff2＝（Ｊm2＊ＪL2／Ｋ2＊ｓ4＋（Ｊm2＋ＪL2）ｓ2）＊ｘref2
−（Ｊm2／Ｋ2＊ｓ2＋１）＊ｄis1・・・（２１）
１７は２軸目干渉力計算部であり、２軸目位置指令ｘref2を入力し、２軸目から１軸目へ作用する干渉力ｄis2を計算し出力する。図４から分かるように、ｄis2は式（２２）で計算される。
ｄis2＝Ａ＊ｘref2＊ｓ2・・・（２２）
最終的に２軸目ロボットの制御装置２００からモータ１８ヘ出力される２軸目トルク指令値ｔref2は以下の式（２３）で計算される。
ｔref2＝ｔref_ff2＋ｔref_fb2・・・（２３）
【００２７】
このように、干渉力が作用しても、各軸アーム９，１９が各軸指令通りに動作するように、モータ位置信号と、モータトルク指令信号を作成し、それらの通りモータ８，１８が動作するようにフィードバック制御を行う構成にしているので、軸間干渉があった場合も、各軸アーム９，１９は自軸の指令通りに動作することができる。
本発明が特許文献１と異なる部分は、擬似モデルやモデル用の制御器を有さず、非干渉位置信号作成部と非干渉トルク信号作成部とを備え、モータトルク補正信号だけではなく、モータ位置信号も干渉を考慮して作成する部分である。
【実施例２】
【００２８】
図２は第２実施例の構成を示す図である。本実施例が実施例１と異なる点は、実施例１では２軸の干渉を考慮していたのに対し、本実施例では複数軸間に干渉力が作用する場合を考慮している点である。
図２に示すように、ｎ軸で構成されたロボットに関して考える。ここでは、２軸目からｎ軸目それぞれから１軸目へ干渉が作用している場合の１軸目の非干渉化を例に取り説明するが、他の軸間の干渉に関しても全く同様に実現できる。
図中、３８はｎ軸目モータを表しｎ軸目ロボットの制御装置３００から出力されるｎ軸目トルク指令ｔrefnにより動作する。３９はｎ軸目アームであり、モータ３８と減速機等を介して結合されている。３０はｎ軸目モータ３８の位置ｘfbnを検出するｎ軸目モータ位置検出器である。
５００は干渉力加算器であり、２軸目から１軸目に作用する干渉力計算値ｄis2からｎ軸目から１軸目に作用する干渉力計算値ｄisnまでの干渉力の総和ｄis_allを求める処理を式（２４）に示すように行う。
ｄis_all＝ｄis2＋ｄis3＋・・・＋ｄisn・・・（２４）
ここで、ｄis2からｄisnまでは、実施例１で図３を用いて説明したのと同様に、１軸目からｎ軸目までの各軸の構成、成す角度、幾何学的関係から求まる。
【００２９】
このように、複数軸間に干渉が作用する場合でも、それぞれの軸から自軸へ作用する干渉力計算値の合計値を入力とし、後は、実施例１と同様の処理を行うだけで、簡単な構成で計算量も少なくすべての軸間の非干渉化を行うことが可能である。
また、軸が増えた場合や、考慮していなかった干渉を新たに考慮する場合なども、各軸の処理は全く変更することなく、各軸間の干渉力計算値の和を計算する部分を追加修正するだけなので、簡単に対応できる。
【産業上の利用可能性】
【００３０】
本発明は、複数軸間で干渉力が作用しても、計算量は少なく、ロボットの非干渉化が実現できるため、軸数の多いロボットの非干渉化制御や、安価なＣＰＵを用いたロボットの制御装置などの用途にも使用できる。
【図面の簡単な説明】
【００３１】
【図１】本発明の第１実施例の構成を示すブロック図である。
【図２】本発明の第２実施例の構成を示すブロック図である。
【図３】干渉力の計算に必要な係数Ａ，Ｂを求める際の力学モデルである。
【図４】単軸を２慣性系とし、互いの干渉力を考慮した制御対象モデルである。
【図５】従来のロボット制御装置の構成を示すブロック図である。
【図６】従来のロボット制御装置のモデル制御器内部の処理を示すブロック図である。
【符号の説明】
【００３２】
１  位置制御部
２  速度制御部
３  微分器
４  微分器
５  非干渉位置信号作成部
６  非干渉トルク信号作成部
７  干渉力計算部
８  モータ
９  アーム
１０  位置検出器
１１  位置制御部
１２  速度制御部
１３  微分器
１４  微分器
１５  非干渉位置信号作成部
１６  非干渉トルク信号作成部
１７  干渉力計算部
１８  モータ
１９  アーム
２０  位置検出器
３８  モータ
３９  アーム
３０  位置検出器
１００  １軸目ロボットの制御装置
２００  ２軸目ロボットの制御装置
３００  ｎ軸目ロボットの制御装置
５００  干渉力加算器
５１  モデル制御器
【図１】
2005186235.tif 000020
【図２】
2005186235.tif 000021
【図３】
2005186235.tif 000022
【図４】
2005186235.tif 000023
【図５】
2005186235.tif 000024
【図６】
2005186235.tif 000025