JP4192780B2 - ロボットの制御装置 - Google Patents

Description

本発明は、互いに干渉する複数軸で構成されるロボットを、干渉力が作用しても各軸が指令どおり動作するように、制御を行うロボットの制御装置に関する。

従来のロボット制御装置は、モデル制御器を有し、モデル制御器内部では、擬似モデルと、擬似モデル用の制御部と、干渉トルクを相殺するためにモデル補償トルクを算出する補正量算出部とを有し、モデル補償トルクが加算されたモデルフィードフォワード指令を、フィードフォワード信号として出力することで、非干渉化を行っている（例えば、特許文献１参照）。
図５は特許文献１に開示された従来の実施の一形態のロボット制御装置の構成を示す制御ブロック図であり、図６は、図５のモデル制御器の構成を示す制御ブロック図である。図５では、各軸ごとに２慣性系（電動機＋減速機等のバネ＋アーム）に近似した２軸のロボット制御系の基本構成を示している。図５および図６では、２軸をＬ軸、Ｕ軸とし、Ｌ軸に関する指令や量についてはそのサフィックスに“＿Ｌ”付加し、Ｕ軸に関する指令や量についてはそのサフィックスに“＿Ｕ”を付加している。

このロボット制御装置は、各軸ごとの電動機位置指令Ｘ_{ref_L}，Ｘ_{ref_U}に基づいてＬ軸及びＵ軸の制御を行うものであり、２軸のフィードバック制御系（フィードバック制御器）５３Ｌ，５３Ｕと制御対象（電動機、減速機、ロボットアーム）との擬似モデルであるモデル制御器５１を有している。モデル制御器５１は、電動機位置指令Ｘ_{ref_L}，Ｘ_{ref_U}を入力とし、ロボットのダイナミクスを考慮して各軸のフィードフォワード指令Ｕ_{FF_L}，Ｕ_{FF_U}を計算し、各軸の電動機の加速度項へのフィードフォワード補償を行うとともに、規範となるモデルにおける各軸の電動機位置θ_{Mm_L}，θ_{Mm_U}、電動機速度

ねじれ角θ_{Ms_L}，θ_{Ms_U}、ねじれ角速度

を算出して出力する。

以下、モデル制御器５１の出力である各要素には、接頭語「モデル」を付加する。
一方、フィードバック制御系（フィードバック制御器）５３Ｌ，５３Ｕにおいて、各軸の位置ゲインはＫ_{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}であり、電動機の実際の速度は

であり、電動機の実際の加速度は

である。
さらに減速機での減速比がＮ_{_L}，Ｎ_{_U}、減速機のバネ定数がＫ_{c_L}，Ｋ_{c_U}、アームの慣性モーメントがＪ_{L_L}，Ｊ_{L_U}、アームの実際の位置がθ_{L_L}，θ_{L_U}、アームの加速度（負荷加速度）が

で表わされている。そして、両軸間の干渉を表わすために、ロボットアームの質量や両軸間の角度により決定されるＭ_ＬＵ／Ｍ_ｏ ^２の値がＬ軸の負荷トルクに乗じられてＵ軸の負荷加速度に作用し、同様に、Ｍ_ＵＬ／Ｍ_ｏ ^２の値がＵ軸の負荷トルクに乗じられてＬ軸の負荷加速度に作用する構成となっている。

このロボット制御系には、Ｌ軸、Ｕ軸のそれぞれごとに、電動機への最終的な加速度指令Ｕ_{ref_L}，Ｕ_{ref_U}と電動機の実際の位置θ_{m_L}，θ_{m_U}を入力として、実際のねじれ角θ_{s_L}，θ_{s_U}とその角速度

とを推定する状態観測器５２Ｌ，５２Ｕが設けられている。さて、このフィードバック制御器では、モデル制御器５１から出力されるモデル電動機位置θ_{Mm_L}，θ_{Mm_U}と実際の電動機位置θ_{m_L}，θ_{m_U}との偏差に対して位置ゲインＫ_{p_L}，Ｋ_{p_U}を乗算して、それぞれの軸の速度指令としている。
このように得られた各速度指令に、モデル電動機速度

と実際の電動機速度

との偏差を加算し、速度ゲインＫ_{v_L}，Ｋ_{v_U}を乗算して、それぞれ、加速度指令としている。

このような加速度指令に対して、（ａ）モデル制御器５１からのフィードフォワード指令Ｕ_{FF_L}，Ｕ_{FF_U}と、（ｂ）モデルねじれ角θ_{Ms_L}，θ_{Ms_U}と状態観測器５２Ｌ，５２Ｕから出力されるねじれ角θ_{s_L}，θ_{s_U}との偏差にフィードバックゲインＫ_{1_L}，Ｋ_{1_U}を乗じたものと、（ｃ）モデルねじれ角速度

と状態観測器５２Ｌ，５２Ｕから出力されるねじれ角速度

との偏差にフィードバックゲインＫ_{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}を乗じて各軸のモデル速度指令としている。そして、これらのモデル速度指令から各軸のモデル電動機速度

を減算した値にモデル速度ゲインＫ_{vM_L}，Ｋ_{vM_U}を乗算して、それぞれ各軸のモデル加速度指令としている。

このモデル加速度指令に、モデル減速機から得られるモデルねじれ角θ_{Ms_L}，θ_{Ms_U}にモデルフィードバックゲインＫ_{1M_L}，Ｋ_{1M_U}を乗じて得られる値と、モデルねじれ角θ_{Ms_L}，θ_{Ms_U}を微分して得られるモデルねじれ角速度

にモデルフィードバックゲインＫ_{2M_L}，Ｋ_{2M_U}を乗じて得られる値とを減算し、モデル加速度指令Ｕ_{Mref_L}，Ｕ_{Mref_L}としている。このモデル加速度指令Ｕ_{Mref_L}，Ｕ_{Mref_U}が、モデル電動機に入力されている。
したがって、モデル加速度指令Ｕ_{Mref_L}，Ｕ_{Mref_U}は、

と表わすことができる。
しかし、ここで想定している２軸ロボットでは、干渉により、Ｌ軸の負荷トルクに、ロボットアームの質量や両軸間の角度により決定される値Ｍ_LUM／Ｍ_oM ^２が乗じられて、Ｕ軸の負荷加速度に作用する。同様に、Ｕ軸の負荷トルクに、値Ｍ_ULM／Ｍ_oM ^２が乗じられて、Ｌ軸の負荷加速度に作用する。

このような干渉をアーム側に作用する外乱としてとらえると、Ｌ軸に関しては、下式に示すモデル補正トルクＴ_{comp_L}をモデル電動機加速度指令Ｕ_{Mref_L}に加算すれば、干渉によるアームの振動を軽減することができる。

ここで、Ｄ_{is_UL}は、Ｕ軸からＬ軸に作用する干渉トルクであって、

と表わすことができる。よって、最終的なモデル電動機加速度指令Ｕ_{FF_L}を
Ｕ_{FF_L}＝Ｕ_{Mref_L}＋Ｔ_{comp_L}（５）
として、電動機の加速度項へフィードフォワード補償を行う。このモデル制御器５１には、モデルねじれ角θ_{Ms_U}を入力として上述の式（３），（４）に基づいてモデル補正トルクＴ_{comp_L}を算出する補正量演算部５４Ｌが設けられており、補正量演算部５４Ｌからのモデル補正トルクＴ_{comp_L}がモデル電動機加速度指令Ｕ_{Mref_L}に加算される。

同様に、Ｕ軸についても、モデル補正トルクＴ_{comp_U}を

のように定める。ここでＤ_{is_LU}は、Ｌ軸からＵ軸に作用する干渉トルクであって、

である。モデルねじれ角θ_{Ms_L}を入力として式（６），（７）に基づきモデル補正トルクＴ_{comp_U}を求める補正量演算部５４Ｕが設けられている。その結果、最終的なモデル電動機加速度指令Ｕ_{FF_U}は、
Ｕ_{FF_U}＝Ｕ_{Mref_U}＋Ｔ_{comp_U} （８）
となる。

以上のように、特許文献１に開示された従来技術においては、このようにモデル制御器５１により干渉トルク補正を行った上で、規範となる各状態量を指令としてフィードバック制御系に入力するので、各ロボットアームは、他軸からの干渉による影響を受けなくなる。
このように、従来のロボット制御装置は、他軸から受ける干渉力をモデル制御器で補正し、それを基に規範となる各状態量をフィードバック制御器に入力することにより、補正トルクにノイズ成分が重畳することがなくなり、ロボットアームは干渉による振動を受けずにツール先端の軌跡精度を向上させるのである。

特開平１０−３２９０６３号公報（第２−５頁、図１、図２）

しかしながら、従来のロボット制御装置は、この制御器を実現するために、フィードバック制御部の演算以外に、モデル制御器５１内で、制御周期毎に、ねじれ角の計算など擬似モデルの計算を行う必要があり各軸毎の計算量が膨大になるという問題があった。
また、モデル制御器５１内で、フィードバック制御部の演算以外に、制御周期毎に、擬似モデルを制御するためのモデル用のフィードバック制御の計算を行う必要があり各軸毎の計算量がさらに増えるという問題がある。
また、非干渉化をトルクの補正のみで行うため、モデル補正トルクＴ_compの計算にモデル位置ゲインＫ_pMやモデル速度ゲインＫ_vMの値まで必要となり、さらに計算が複雑になるという問題もある。
そして、結果として、各軸毎の計算量が膨大になり、制御演算時間が長くなり、本制御装置を実現するために高速なＣＰＵを必要とするという問題がある。
本発明はこのような問題点に鑑みてなされたものであり、計算量を大幅に低減してＣＰＵの負担を軽減することのできるロボットの制御装置を提供することを目的とする。

上記問題を解決するため、本発明の第１ の構成は、互いに干渉する複数軸から構成されるロボットであって、モータと、前記モータに減速機等を介して結合されたアームと、前記モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置において、自軸の指令から他軸に作用する干渉力を求める干渉力計算部と、他軸から作用する干渉力がある場合も前記アーム先端が自軸の指令どおり動作するような非干渉トルク指令を、自軸の指令と他軸から作用する干渉力から多慣性系干渉モデルを用いて求める非干渉トルク信号作成部と、他軸から作用する干渉力がある場合も前記アーム先端が自軸の指令どおり動作するような非干渉位置指令を、自軸の指令と他軸から作用する干渉力から前記多慣性系干渉モデルを用いて求める非干渉位置信号作成部とを備え、前記非干渉位置指令どおりに前記モータが動作するように制御演算してトルク指令を作成し、前記トルク指令に前記非干渉トルク指令を加算した指令を前記モータに出力することを特徴とするものである。
この第１ の構成においては、干渉力計算部と非干渉トルク信号作成部と非干渉位置信号作成部を有し、擬似モデルの計算と擬似モデル用の制御の計算は行わず、且つ、モータトルク信号だけではなく、モータ位置信号も干渉を考慮して作成することで、計算量が大幅に低減する。

本発明の第２の構成は、軸が２軸である場合の干渉について、前記干渉力計算部における干渉力は、次式
ｄ_is1＝Ｂ＊ｘ_ref1＊ｓ²
ｄ_is2＝Ａ＊ｘ_ref2＊ｓ²
によって計算し、
前記非干渉トルク信号作成部における非干渉トルク信号は、次式
ｔ_{ref_ff1}＝（Ｊ_m1＊Ｊ_L1／Ｋ1＊ｓ⁴＋（Ｊ_m1＋Ｊ_L1）ｓ²）＊ｘ_ref1
−（Ｊ_m1／Ｋ1＊ｓ²＋１）＊ｄ_is2
ｔ_{ref_ff2}＝（Ｊ_m2＊Ｊ_L2／Ｋ2＊ｓ⁴＋（Ｊ_m2＋Ｊ_L2）ｓ²）＊ｘ_ref2
−（Ｊ_m2／Ｋ2＊ｓ²＋１）＊ｄ_is1
によって計算し、
前記非干渉位置信号作成部における非干渉位置信号は、次式
ｘ_{ref_ff1}＝（Ｊ_L1／Ｋ1＊ｓ²＋１）＊ｘ_ref1−１／Ｋ1＊ｄ_is2
ｘ_{ref_ff2}＝（Ｊ_L2／Ｋ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＊Ｌ2²・・・（９）

以下、実際に１軸目ロボットの制御装置１００内部の構成を説明する。
図１において、１は１軸目の位置制御部であり、１軸目非干渉位置信号ｘ_{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＊ｓ²＋１）＊ｘ_ref1−１／Ｋ1＊ｄ_is2 ・・・（１３）

６は１軸目非干渉トルク信号作成部であり、１軸目位置指令ｘ_ref1と２軸目から１軸目へ作用する干渉力計算値ｄ_is2を入力し、干渉力ｄ_is2の影響が作用しても、１軸目位置指令ｘ_ref1通りにアーム９先端ｘ１が動作するための１軸目モータトルク指令信号を図４に示す２軸の干渉モデルの関係を用いて作成し、１軸目非干渉トルク信号ｔ_{ref_ff1}として出力する。ｔ_{ref_ff1}の計算式を式（１４）に示す。
ｔ_{ref_ff1}＝（Ｊ_m1＊Ｊ_L1／Ｋ1＊ｓ⁴＋（Ｊ_m1＋Ｊ_L1）ｓ²）＊ｘ_ref1
−（Ｊ_m1／Ｋ1＊ｓ²＋１）＊ｄ_is2・・・（１４）
７は１軸目干渉力計算部であり、１軸目位置指令ｘ_ref1を入力し、１軸目から２軸目へ作用する干渉力ｄ_is1を計算し出力する。図４から分かるように、ｄ_is1は式（１５）で計算される。
ｄ_is1＝Ｂ＊ｘ_ref1＊ｓ²・・・（１５）
最終的に１軸目ロボットの制御装置１００からモータ８ヘ出力される１軸目トルク指令値ｔ_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＊ｓ²＋１）＊ｘ_ref2−１／Ｋ2＊ｄ_is1 ・・・（２０）

１６は２軸目非干渉トルク信号作成部であり、２軸目位置指令ｘ_ref2と１軸目から２軸目へ作用する干渉力計算値ｄ_is1を入力し、干渉力ｄ_is1の影響が作用しても、２軸目位置指令ｘ_ref2通りにアーム１９先端ｘ２が動作するための２軸目モータトルク指令信号を図４に示す２軸の干渉モデルの関係を用いて作成し、２軸目非干渉トルク信号ｔ_{ref_ff2}として出力する。ｔ_{ref_ff2}の計算式を式（２１）に示す。
ｔ_{ref_ff2}＝（Ｊ_m2＊Ｊ_L2／Ｋ2＊ｓ⁴＋（Ｊ_m2＋Ｊ_L2）ｓ²）＊ｘ_ref2
−（Ｊ_m2／Ｋ2＊ｓ²＋１）＊ｄ_is1・・・（２１）
１７は２軸目干渉力計算部であり、２軸目位置指令ｘ_ref2を入力し、２軸目から１軸目へ作用する干渉力ｄ_is2を計算し出力する。図４から分かるように、ｄ_is2は式（２２）で計算される。
ｄ_is2＝Ａ＊ｘ_ref2＊ｓ²・・・（２２）
最終的に２軸目ロボットの制御装置２００からモータ１８ヘ出力される２軸目トルク指令値ｔ_ref2は以下の式（２３）で計算される。
ｔ_ref2＝ｔ_{ref_ff2}＋ｔ_{ref_fb2}・・・（２３）

このように、干渉力が作用しても、各軸アーム９，１９が各軸指令通りに動作するように、モータ位置信号と、モータトルク指令信号を作成し、それらの通りモータ８，１８が動作するようにフィードバック制御を行う構成にしているので、軸間干渉があった場合も、各軸アーム９，１９は自軸の指令通りに動作することができる。
本発明が特許文献１と異なる部分は、擬似モデルやモデル用の制御器を有さず、非干渉位置信号作成部と非干渉トルク信号作成部とを備え、モータトルク補正信号だけではなく、モータ位置信号も干渉を考慮して作成する部分である。

図２は第２実施例の構成を示す図である。本実施例が実施例１と異なる点は、実施例１では２軸の干渉を考慮していたのに対し、本実施例では複数軸間に干渉力が作用する場合を考慮している点である。
図２に示すように、ｎ軸で構成されたロボットに関して考える。ここでは、２軸目からｎ軸目それぞれから１軸目へ干渉が作用している場合の１軸目の非干渉化を例に取り説明するが、他の軸間の干渉に関しても全く同様に実現できる。
図中、３８はｎ軸目モータを表しｎ軸目ロボットの制御装置３００から出力されるｎ軸目トルク指令ｔ_refnにより動作する。３９はｎ軸目アームであり、モータ３８と減速機等を介して結合されている。３０はｎ軸目モータ３８の位置ｘ_fbnを検出するｎ軸目モータ位置検出器である。
５００は干渉力加算器であり、２軸目から１軸目に作用する干渉力計算値ｄ_is2からｎ軸目から１軸目に作用する干渉力計算値ｄ_isnまでの干渉力の総和ｄ_{is_all}を求める処理を式（２４）に示すように行う。
ｄ_{is_all}＝ｄ_is2＋ｄ_is3＋・・・＋ｄ_isn・・・（２４）
ここで、ｄ_is2からｄ_isnまでは、実施例１で図３を用いて説明したのと同様に、１軸目からｎ軸目までの各軸の構成、成す角度、幾何学的関係から求まる。

このように、複数軸間に干渉が作用する場合でも、それぞれの軸から自軸へ作用する干渉力計算値の合計値を入力とし、後は、実施例１と同様の処理を行うだけで、簡単な構成で計算量も少なくすべての軸間の非干渉化を行うことが可能である。
また、軸が増えた場合や、考慮していなかった干渉を新たに考慮する場合なども、各軸の処理は全く変更することなく、各軸間の干渉力計算値の和を計算する部分を追加修正するだけなので、簡単に対応できる。

本発明は、複数軸間で干渉力が作用しても、計算量は少なく、ロボットの非干渉化が実現できるため、軸数の多いロボットの非干渉化制御や、安価なＣＰＵを用いたロボットの制御装置などの用途にも使用できる。

本発明の第１実施例の構成を示すブロック図である。 本発明の第２実施例の構成を示すブロック図である。 干渉力の計算に必要な係数Ａ，Ｂを求める際の力学モデルである。 単軸を２慣性系とし、互いの干渉力を考慮した制御対象モデルである。 従来のロボット制御装置の構成を示すブロック図である。 従来のロボット制御装置のモデル制御器内部の処理を示すブロック図である。

符号の説明

１ 位置制御部
２ 速度制御部
３ 微分器
４ 微分器
５ 非干渉位置信号作成部
６ 非干渉トルク信号作成部
７ 干渉力計算部
８ モータ
９ アーム
１０ 位置検出器
１１ 位置制御部
１２ 速度制御部
１３ 微分器
１４ 微分器
１５ 非干渉位置信号作成部
１６ 非干渉トルク信号作成部
１７ 干渉力計算部
１８ モータ
１９ アーム
２０ 位置検出器
３８ モータ
３９ アーム
３０ 位置検出器
１００ １軸目ロボットの制御装置
２００ ２軸目ロボットの制御装置
３００ ｎ軸目ロボットの制御装置
５００ 干渉力加算器
５１ モデル制御器

Claims (3)

1. 互いに干渉する複数軸から構成されるロボットであって、モータ（８）（１８）と、前記モータ（８）（１８）に減速機等を介して結合されたアーム（９）（１９）と、前記モータ（８）（１８）の位置を検出するモータ位置検出器とから構成された各軸を、各軸の指令通りに動作させるための位置制御部（１）（１１）および速度制御部（２）（１２）を備えたロボットの制御装置において、
   自軸の指令から他軸に作用する干渉力を求める干渉力計算部（７）（１７）と、
   他軸から作用する干渉力がある場合も前記アーム（９）（１９）先端が自軸の指令どおり動作するような非干渉トルク指令（tref_ff1）（tref_ff2）を、自軸の指令と他軸から作用する干渉力から多慣性系干渉モデルを用いて求める非干渉トルク信号作成部（６）（１６）と、
   他軸から作用する干渉力がある場合も前記アーム（９）（１９）先端が自軸の指令どおり動作するような非干渉位置指令（Xref_ff1）（Xref_ff2）を、自軸の指令と他軸から作用する干渉力から前記多慣性系干渉モデルを用いて求める非干渉位置信号作成部（５）（１５）とを備え、
   前記非干渉位置指令（Xref_ff1）（Xref_ff2）どおりに前記モータ（８）（１８）が動作するように制御演算してトルク指令（tref_fb1）（tref_fb2）を作成し、前記トルク指令（tref_fb1）（tref_fb2）に前記非干渉トルク指令（tref_ff1）（tref_ff2）を加算した指令（tref1）（tref2）を前記モータ（８）（１８）に出力することを特徴とするロボットの制御装置。
   
2. 軸が２ 軸である場合の干渉について、
   前記干渉力計算部における干渉力は、次式
   ｄ i s 1 ＝ Ｂ ＊ ｘ r e f 1 ＊ ｓ 2
   ｄ i s 2 ＝ Ａ ＊ ｘ r e f 2 ＊ ｓ 2
   によって計算し、
   前記非干渉トルク信号作成部における非干渉トルク信号は、次式
   ｔ r e f _ f f 1 ＝ （ Ｊ m 1 ＊ Ｊ L 1 ／ Ｋ 1＊ ｓ 4 ＋ （ Ｊ m 1 ＋ Ｊ L 1 ） ｓ 2 ） ＊ ｘ r e f 1
   − （ Ｊ m 1 ／ Ｋ 1＊ ｓ 2 ＋ １ ） ＊ ｄ i s 2
   ｔ r e f _ f f 2 ＝ （ Ｊ m 2 ＊ Ｊ L 2 ／ Ｋ 2＊ ｓ 4 ＋ （ Ｊ m 2 ＋ Ｊ L 2 ） ｓ 2 ） ＊ ｘ r e f 2
   − （ Ｊ m 2 ／ Ｋ 2＊ ｓ 2 ＋ １ ） ＊ ｄ i s 1
   によって計算し、
   前記非干渉位置信号作成部における非干渉位置信号は、次式
   ｘ r e f _ f f 1 ＝ （ Ｊ L 1 ／ Ｋ 1＊ ｓ 2 ＋ １ ） ＊ ｘ r e f 1 − １ ／ Ｋ 1＊ ｄ i s 2
   ｘ r e f _ f f 2 ＝ （ Ｊ L 2 ／ Ｋ 2＊ ｓ 2 ＋ １ ） ＊ ｘ r e f 2 − １ ／ Ｋ 2＊ ｄ i s 1
   によって計算することを特徴とする請求項１ 記載のロボットの制御装置。
   但し、Ｊ m 1 ： １ 軸目モータ慣性モーメント
   Ｊ L 1 ： １ 軸目アーム慣性モーメント
   Ｋ 1： １ 軸目減速機ばね定数
   Ｊ m 2 ： ２ 軸目モータ慣性モーメント
   Ｊ L 2 ： ２ 軸目アーム慣性モーメント
   Ｋ 2： ２ 軸目減速機ばね定数
   Ａ （ ＝ Ｂ ） ： ２ 軸の構成、成す角度、幾何学的関係から求まる係数
   ｓ ： ラプラス演算子
   ｄ i s 1 ： １ 軸目から２ 軸目に作用する干渉力
   ｄ i s 2 ： ２ 軸目から１ 軸目に作用する干渉力
   ｘ r e f 1 ： １ 軸目位置指令
   ｘ r e f 2 ： ２ 軸目位置指令
3. 前記、非干渉トルク信号作成部および非干渉位置信号作成部の処理で使用する、他軸か
   ら作用する干渉力の計算値は、軸が２ 軸を超える場合、各軸から自軸へ作用する干渉力計
   算値の総和であることを特徴とする請求項１ 記載のロボットの制御装置。

Images